linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
bdc988f164962c603ce6fffd96e3b51ef61c405c
sics/tasdrive.c
koennecke 7d8ad7392c - Fixed a couple of small things with the TAS code 
- Fixed new AMOR settings module
- Initial implementation of the new SICS hierarchical parameter database


SKIPPED:
	psi/amorset.c
	psi/libpsi.a
	psi/sps.c
2006-06-23 13:04:52 +00:00

794 lines
23 KiB
C
Raw Blame History

/*---------------------------------------------------------------------
SICS interface to the triple axis drivable motors.
copyright: see file COPYRIGHT
Mark Koennecke, May 2005
--------------------------------------------------------------------*/
#include <assert.h>
#include "sics.h"
#include "tasdrive.h"
#define ABS(x) (x < 0 ? -(x) : (x))
#define MOTPREC .01
/*------------------- motor indexes in motor data structure ---------
WARNING: have to match tasub.c !!!!!
--------------------------------------------------------------------*/
#define A1 0
#define A2 1
#define MCV 2
#define MCH 3
#define A3 4
#define A4 5
#define SGU 6
#define SGL 7
#define A5 8
#define A6 9
#define ACV 10
#define ACH 11
/*============== Drivable Interface functions =====================*/
static long TASSetValue(void *pData, SConnection *pCon,
float value){
ptasMot self = (ptasMot)pData;
assert(self);
setTasPar(&self->math->target,self->math->tasMode,self->code,value);
self->math->mustDrive = 1;
return OKOK;
}
/*----------------------------------------------------------------*/
static int readTASMotAngles(ptasUB self, SConnection *pCon,
ptasAngles ang){
int status;
float val, theta;
/*
Monochromator
*/
status = MotorGetSoftPosition(self->motors[A1],pCon,&val);
if(status == 0){
return status;
}
theta = val;
status = MotorGetSoftPosition(self->motors[A2],pCon,&val);
if(status == 0){
return status;
}
ang->monochromator_two_theta = val;
if(ABS(val/2. - theta) > .1){
SCWrite(pCon,"WARNING: theta monochromator not half of two theta",eWarning);
}
/*
Analyzer
*/
if(self->tasMode != ELASTIC){
status = MotorGetSoftPosition(self->motors[A5],pCon,&val);
if(status == 0){
return status;
}
theta = val;
status = MotorGetSoftPosition(self->motors[A6],pCon,&val);
if(status == 0){
return status;
}
ang->analyzer_two_theta = val;
if(ABS(val/2. - theta) > .1){
SCWrite(pCon,"WARNING: theta analyzer not half of two theta",eWarning);
}
} else {
ang->analyzer_two_theta = ang->monochromator_two_theta;
}
/*
crystal
*/
status = MotorGetSoftPosition(self->motors[A3],pCon,&val);
if(status == 0){
return status;
}
ang->a3 = val;
status = MotorGetSoftPosition(self->motors[A4],pCon,&val);
if(status == 0){
return status;
}
ang->sample_two_theta = val;
status = MotorGetSoftPosition(self->motors[SGU],pCon,&val);
if(status == 0){
return status;
}
ang->sgu = val;
status = MotorGetSoftPosition(self->motors[SGL],pCon,&val);
if(status == 0){
return status;
}
ang->sgl = val;
return 1;
}
/*-------------------------------------------------------------*/
static float TASGetValue(void *pData, SConnection *pCon){
ptasMot self = (ptasMot)pData;
int status;
tasAngles angles;
double val;
assert(self);
if(self->math->mustRecalculate == 1) {
status = readTASMotAngles(self->math, pCon, &angles);
if(status != 1){
return -999.99;
}
status = calcTasQEPosition(&self->math->machine, angles, &self->math->current);
if(status < 0){
SCWrite(pCon,"ERROR: out of memory calculating Q-E variables",eError);
return -999.99;
}
if(self->math->tasMode == ELASTIC){
self->math->current.kf = self->math->current.ki;
}
self->math->mustRecalculate = 0;
}
val = getTasPar(self->math->current,self->code);
return (float)val;
}
/*-----------------------------------------------------------------*/
static float TASQMGetValue(void *pData, SConnection *pCon){
ptasMot self = (ptasMot)pData;
int status;
tasAngles angles;
double val;
assert(self);
if(self->math->mustRecalculate == 1) {
status = readTASMotAngles(self->math, pCon, &angles);
if(status != 1){
return -999.99;
}
status = calcTasPowderPosition(&self->math->machine,
angles, &self->math->current);
if(status < 0){
SCWrite(pCon,"ERROR: out of memory calculating Q-E variables",eError);
return -999.99;
}
if(self->math->tasMode == ELASTIC){
self->math->current.kf = self->math->current.ki;
}
self->math->mustRecalculate = 0;
}
val = getTasPar(self->math->current,self->code);
return (float)val;
}
/*---------------------------------------------------------------------------------*/
static int TASCheckLimits(void *pData, float fVal, char *error, int iErrLen){
/*
There is no meaningful implementation here. This gets called when starting the motor.
At that stage not all other values may be known. If the calculation fails,
this will die at status check time.
*/
return 1;
}
/*---------------------------------------------------------------------------------*/
static int TASHalt(void *pData){
ptasMot self = (ptasMot)pData;
int i, length = 12;
assert(self);
/**
* disregard non existing analyzer motors
*/
if(self->math->tasMode == ELASTIC){
length = 8;
}
for(i = 0; i < length; i++){
if(self->math->motors[i] != NULL){
self->math->motors[i]->pDrivInt->Halt(self->math->motors[i]);
}
}
return 1;
}
/*--------------------------------------------------------------------------*/
static void writeMotPos(SConnection *pCon, int silent, char *name,
float val, float target){
char pBueffel[132];
if(silent != 1) {
snprintf(pBueffel,131,"Driving %5s from %8.3f to %8.3f",
name, val, target);
SCWrite(pCon,pBueffel,eWarning);
}
}
/*---------------------------------------------------------------------------*/
static int startMotors(ptasMot self, tasAngles angles,
SConnection *pCon, int driveQ, int driveTilt){
float val;
double curve;
int status, silent;
silent = self->math->silent;
/*
monochromator
*/
val = self->math->motors[A1]->pDrivInt->GetValue(self->math->motors[A1],pCon);
if(ABS(val - angles.monochromator_two_theta/2.) > MOTPREC){
status = self->math->motors[A1]->pDrivInt->SetValue(self->math->motors[A1],
pCon,
angles.monochromator_two_theta/2.);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"a1",val, angles.monochromator_two_theta/2.);
val = self->math->motors[A2]->pDrivInt->GetValue(self->math->motors[A2],pCon);
if(ABS(val - angles.monochromator_two_theta) > MOTPREC){
status = self->math->motors[A2]->pDrivInt->SetValue(self->math->motors[A2],
pCon,
angles.monochromator_two_theta);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"a2",val, angles.monochromator_two_theta);
if(self->math->motors[MCV] != NULL){
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta);
val = self->math->motors[MCV]->pDrivInt->GetValue(self->math->motors[MCV],pCon);
if(ABS(val - curve) > MOTPREC){
status = self->math->motors[MCV]->pDrivInt->SetValue(self->math->motors[MCV],
pCon,
curve);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"mcv",val, curve);
}
if(self->math->motors[MCH] != NULL){
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta);
val = self->math->motors[MCH]->pDrivInt->GetValue(self->math->motors[MCH],pCon);
if(ABS(val - curve) > MOTPREC){
status = self->math->motors[MCH]->pDrivInt->SetValue(self->math->motors[MCH],
pCon,
curve);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"mch",val, curve);
}
/*
analyzer
*/
if(self->math->tasMode != ELASTIC){
val = self->math->motors[A5]->pDrivInt->GetValue(self->math->motors[A5],
pCon);
if(ABS(val - angles.analyzer_two_theta/2.) > MOTPREC){
status = self->math->motors[A5]->pDrivInt->SetValue(self->math->motors[A5],
pCon,
angles.analyzer_two_theta/2.);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,self->math->motors[A5]->name,
val, angles.analyzer_two_theta/2.);
val = self->math->motors[A6]->pDrivInt->GetValue(self->math->motors[A6],pCon);
if(ABS(val - angles.analyzer_two_theta) > MOTPREC){
status = self->math->motors[A6]->pDrivInt->SetValue(self->math->motors[A6],
pCon,
angles.analyzer_two_theta);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"a6",val, angles.analyzer_two_theta);
if(self->math->motors[ACV] != NULL){
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta);
val = self->math->motors[ACV]->pDrivInt->GetValue(self->math->motors[ACV],pCon);
if(ABS(val - curve) > MOTPREC){
status = self->math->motors[ACV]->pDrivInt->SetValue(self->math->motors[ACV],
pCon,
curve);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"acv",val, curve);
}
if(self->math->motors[ACH] != NULL){
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta);
val = self->math->motors[ACH]->pDrivInt->GetValue(self->math->motors[ACH],
pCon);
if(ABS(val - curve) > MOTPREC){
status = self->math->motors[ACH]->pDrivInt->SetValue(self->math->motors[ACH],
pCon,
curve);
if(status != OKOK){
return status;
}
}
}
writeMotPos(pCon,silent,"ach",val, curve);
}
if(driveQ == 0){
return OKOK;
}
/*
crystal
*/
val = self->math->motors[A3]->pDrivInt->GetValue(self->math->motors[A3],pCon);
if(ABS(val - angles.a3) > MOTPREC){
status = self->math->motors[A3]->pDrivInt->SetValue(self->math->motors[A3],
pCon,
angles.a3);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"a3",val, angles.a3);
val = self->math->motors[A4]->pDrivInt->GetValue(self->math->motors[A4],pCon);
if(ABS(val - angles.sample_two_theta) > MOTPREC){
status = self->math->motors[A4]->pDrivInt->SetValue(self->math->motors[A4],
pCon,
angles.sample_two_theta);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"a4",val, angles.sample_two_theta);
if(driveTilt == 1){
val = self->math->motors[SGL]->pDrivInt->GetValue(self->math->motors[SGL],pCon);
if(ABS(val - angles.sgl) > MOTPREC){
status = self->math->motors[SGL]->pDrivInt->SetValue(self->math->motors[SGL],
pCon,
angles.sgl);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"sgl",val, angles.sgl);
val = self->math->motors[SGU]->pDrivInt->GetValue(self->math->motors[SGU],pCon);
if(ABS(val - angles.sgu) > MOTPREC){
status = self->math->motors[SGU]->pDrivInt->SetValue(self->math->motors[SGU],
pCon,
angles.sgu);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,"sgu",val, angles.sgu);
}
self->math->mustDrive = 0;
return OKOK;
}
/*---------------------------------------------------------------------------*/
static int checkQMotorLimits(ptasMot self, SConnection *pCon,
tasAngles angles, int driveTilt){
int status, retVal = 1;
char error[131];
char pBueffel[256];
status = self->math->motors[A3]->pDrivInt->CheckLimits(self->math->motors[A3],
angles.a3,
error,
131);
if(status != 1) {
retVal = 0;
snprintf(pBueffel,256,"ERROR: limit violation an a3: %s", error);
SCWrite(pCon,pBueffel,eError);
}
status = self->math->motors[A4]->pDrivInt->CheckLimits(self->math->motors[A4],
angles.sample_two_theta,
error,
131);
if(status != 1) {
retVal = 0;
snprintf(pBueffel,256,"ERROR: limit violation an a4: %s", error);
SCWrite(pCon,pBueffel,eError);
}
if(driveTilt == 1){
status = self->math->motors[A3]->pDrivInt->CheckLimits(self->math->motors[SGU],
angles.sgu,
error,
131);
if(status != 1) {
retVal = 0;
snprintf(pBueffel,256,"ERROR: limit violation an SGU: %s", error);
SCWrite(pCon,pBueffel,eError);
}
status = self->math->motors[SGL]->pDrivInt->CheckLimits(self->math->motors[SGL],
angles.sgl,
error,
131);
if(status != 1) {
retVal = 0;
snprintf(pBueffel,256,"ERROR: limit violation an SGL: %s", error);
SCWrite(pCon,pBueffel,eError);
}
}
return retVal;
}
/*-----------------------------------------------------------------------------*/
static int calculateAndDrive(ptasMot self, SConnection *pCon){
tasAngles angles;
int status, driveQ = 1, driveTilt = 1;
MATRIX scatteringPlaneNormal = NULL;
if(self->math->ubValid == 0){
SCWrite(pCon,"WARNING: UB matrix invalid",eWarning);
}
status = calcAllTasAngles(&self->math->machine, self->math->target,
&angles);
self->math->mustDrive = 0;
switch(status){
case ENERGYTOBIG:
SCWrite(pCon,"ERROR: desired energy to big",eError);
return HWFault;
break;
case UBNOMEMORY:
SCWrite(pCon,"ERROR: out of memory calculating angles",eError);
driveQ = 0;
break;
case BADRMATRIX:
SCWrite(pCon,"ERROR: bad crystallographic parameters or bad UB",eError);
driveQ = 0;
break;
case BADUBORQ:
SCWrite(pCon,"ERROR: bad UB matrix or bad Q-vector",eError);
driveQ = 0;
break;
case TRIANGLENOTCLOSED:
SCWrite(pCon,"ERROR: cannot close scattering triangle",eError);
driveQ = 0;
break;
}
/**
* check out of plane condition and permission
*/
if(self->math->outOfPlaneAllowed != 1){
scatteringPlaneNormal = calcScatteringPlaneNormal(self->math->r1.qe,
self->math->r2.qe);
if(scatteringPlaneNormal == NULL){
SCWrite(pCon,"ERROR: unable to calculate scattering plane normal",eError);
driveQ = 0;
} else {
if(!isInPlane(scatteringPlaneNormal,self->math->target)){
SCWrite(pCon,
"WARNING: scattering vector is out of plane and you disallowed me to try to drive there",
eWarning);
}
}
driveTilt = 0;
}
if(driveQ != 0){
if(!checkQMotorLimits(self,pCon,angles,driveTilt)){
driveQ = 0;
}
}
if(driveQ == 0){
SCWrite(pCon,"WARNING: NOT driving Q-vector because of errors",eError);
}
return startMotors(self,angles,pCon, driveQ,driveTilt);
}
/*-----------------------------------------------------------------------------*/
static int checkMotors(ptasMot self, SConnection *pCon){
int i, status, length = 12;
self->math->mustRecalculate = 1;
if(self->math->tasMode == ELASTIC){
length = 8;
}
for(i = 0; i < 12; i++){
if(self->math->motors[i] != NULL){
status = self->math->motors[i]->pDrivInt->CheckStatus(self->math->motors[i],
pCon);
if(status != HWIdle && status != OKOK){
return status;
}
}
}
return HWIdle;
}
/*------------------------------------------------------------------------------*/
static int TASGetStatus(void *pData, SConnection *pCon){
ptasMot self = (ptasMot)pData;
int status;
assert(self);
if(self->math->mustDrive == 1){
status = calculateAndDrive(self,pCon);
if(status != OKOK){
return HWFault;
} else {
return HWBusy;
}
} else {
return checkMotors(self, pCon);
}
}
/*---------------------------------------------------------------------------*/
static int startQMMotors(ptasMot self, tasAngles angles,
SConnection *pCon){
float val;
double curve;
int status;
/*
monochromator
*/
val = self->math->motors[A1]->pDrivInt->GetValue(self->math->motors[A1],pCon);
if(ABS(val - angles.monochromator_two_theta/2.) > MOTPREC){
status = self->math->motors[A1]->pDrivInt->SetValue(self->math->motors[A1],
pCon,
angles.monochromator_two_theta/2.);
if(status != OKOK){
return status;
}
}
val = self->math->motors[A2]->pDrivInt->GetValue(self->math->motors[A2],pCon);
if(ABS(val - angles.monochromator_two_theta) > MOTPREC){
status = self->math->motors[A2]->pDrivInt->SetValue(self->math->motors[A2],
pCon,
angles.monochromator_two_theta);
if(status != OKOK){
return status;
}
}
if(self->math->motors[MCV] != NULL){
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta);
val = self->math->motors[MCV]->pDrivInt->GetValue(self->math->motors[MCV],pCon);
if(ABS(val - curve) > MOTPREC){
status = self->math->motors[MCV]->pDrivInt->SetValue(self->math->motors[MCV],
pCon,
curve);
if(status != OKOK){
return status;
}
}
}
if(self->math->motors[MCH] != NULL){
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta);
val = self->math->motors[MCH]->pDrivInt->GetValue(self->math->motors[MCH],pCon);
if(ABS(val - curve) > MOTPREC){
status = self->math->motors[MCH]->pDrivInt->SetValue(self->math->motors[MCH],
pCon,
curve);
if(status != OKOK){
return status;
}
}
}
/*
analyzer
*/
val = self->math->motors[A5]->pDrivInt->GetValue(self->math->motors[A5],pCon);
if(ABS(val - angles.analyzer_two_theta/2.) > MOTPREC){
status = self->math->motors[A5]->pDrivInt->SetValue(self->math->motors[A5],
pCon,
angles.analyzer_two_theta/2.);
if(status != OKOK){
return status;
}
}
val = self->math->motors[A6]->pDrivInt->GetValue(self->math->motors[A6],pCon);
if(ABS(val - angles.analyzer_two_theta) > MOTPREC){
status = self->math->motors[A6]->pDrivInt->SetValue(self->math->motors[A6],
pCon,
angles.analyzer_two_theta);
if(status != OKOK){
return status;
}
}
if(self->math->motors[ACV] != NULL){
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta);
val = self->math->motors[ACV]->pDrivInt->GetValue(self->math->motors[ACV],pCon);
if(ABS(val - curve) > MOTPREC){
status = self->math->motors[ACV]->pDrivInt->SetValue(self->math->motors[ACV],
pCon,
curve);
if(status != OKOK){
return status;
}
}
}
if(self->math->motors[ACH] != NULL){
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta);
val = self->math->motors[ACH]->pDrivInt->GetValue(self->math->motors[ACH],pCon);
if(ABS(val - curve) > MOTPREC){
status = self->math->motors[ACH]->pDrivInt->SetValue(self->math->motors[ACH],
pCon,
curve);
if(status != OKOK){
return status;
}
}
}
/*
crystal
*/
val = self->math->motors[A4]->pDrivInt->GetValue(self->math->motors[A4],pCon);
if(ABS(val - angles.sample_two_theta) > MOTPREC){
status = self->math->motors[A4]->pDrivInt->SetValue(self->math->motors[A4],
pCon,
angles.sample_two_theta);
if(status != OKOK){
return status;
}
}
self->math->mustDrive = 0;
return OKOK;
}
/*-----------------------------------------------------------------------------*/
static int calculateQMAndDrive(ptasMot self, SConnection *pCon){
tasAngles angles;
int status;
status = calcTasPowderAngles(&self->math->machine, self->math->target,
&angles);
switch(status){
case ENERGYTOBIG:
SCWrite(pCon,"ERROR: desired energy to big",eError);
return HWFault;
break;
case TRIANGLENOTCLOSED:
SCWrite(pCon,"ERROR: cannot close scattering triangle",eError);
return HWFault;
break;
default:
return startQMMotors(self,angles,pCon);
}
return HWFault;
}
/*------------------------------------------------------------------------------*/
static int TASQMGetStatus(void *pData, SConnection *pCon){
ptasMot self = (ptasMot)pData;
int status;
assert(self);
if(self->math->mustDrive == 1){
status = calculateQMAndDrive(self,pCon);
if(status != OKOK){
return HWFault;
} else {
return HWBusy;
}
} else {
return checkMotors(self, pCon);
}
}
/*================== Life and Death =========================================*/
static void KillTasMot(void *pData){
ptasMot self = (ptasMot)pData;
if(self == NULL){
return;
}
if(self->pDes != NULL){
DeleteDescriptor(self->pDes);
self->pDes == NULL;
}
if(self->pDriv != NULL){
free(self->pDriv);
self->pDriv = NULL;
}
free(self);
}
/*---------------------------------------------------------------------------*/
static void *GetTasInterface(void *pData, int interfaceID){
ptasMot self = (ptasMot)pData;
if(self != NULL && interfaceID == DRIVEID){
return self->pDriv;
} else {
return NULL;
}
}
/*-------------------------------------------------------------------------*/
int InstallTasMotor(SicsInterp *pSics, ptasUB self, int tasVar, char *name){
ptasMot pNew = NULL;
pNew = (ptasMot)malloc(sizeof(tasMot));
if(!pNew){
return 0;
}
memset(pNew,0,sizeof(tasMot));
pNew->pDes = CreateDescriptor("TasMot");
pNew->pDriv = CreateDrivableInterface();
if(!pNew->pDes || !pNew->pDriv) {
free(pNew);
return 0;
}
pNew->pDes->GetInterface = GetTasInterface;
pNew->pDriv->Halt = TASHalt;
pNew->pDriv->CheckLimits = TASCheckLimits;
pNew->pDriv->SetValue = TASSetValue;
pNew->pDriv->CheckStatus = TASGetStatus;
pNew->pDriv->GetValue = TASGetValue;
pNew->math = self;
pNew->code = tasVar;
return AddCommand(pSics,name, TasMot, KillTasMot,pNew);
}
/*-------------------------------------------------------------------------*/
int InstallTasQMMotor(SicsInterp *pSics, ptasUB self){
ptasMot pNew = NULL;
pNew = (ptasMot)malloc(sizeof(tasMot));
if(!pNew){
return 0;
}
memset(pNew,0,sizeof(tasMot));
pNew->pDes = CreateDescriptor("TasMot");
pNew->pDriv = CreateDrivableInterface();
if(!pNew->pDes || !pNew->pDriv) {
free(pNew);
return 0;
}
pNew->pDes->GetInterface = GetTasInterface;
pNew->pDriv->Halt = TASHalt;
pNew->pDriv->CheckLimits = TASCheckLimits;
pNew->pDriv->SetValue = TASSetValue;
pNew->pDriv->CheckStatus = TASQMGetStatus;
pNew->pDriv->GetValue = TASQMGetValue;
pNew->math = self;
pNew->code = QM;
return AddCommand(pSics,"qm", TasMot, KillTasMot,pNew);
}
/*------------------------------------------------------------------------*/
int TasMot(SConnection *pCon,SicsInterp *pSics, void *pData,
int argc, char *argv[]){
ptasMot self = (ptasMot)pData;
float value;
char pBueffel[132];
if(argc > 1){
strtolower(argv[1]);
if(strcmp(argv[1],"target") == 0){
snprintf(pBueffel,131,"%s.target = %f",
argv[0], getTasPar(self->math->target,self->code));
SCWrite(pCon,pBueffel,eValue);
return 1;
} else {
snprintf(pBueffel,131,"ERROR: tasmot does not understand %s", argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
}
value = self->pDriv->GetValue(self,pCon);
snprintf(pBueffel,131,"%s = %f", argv[0], value);
SCWrite(pCon,pBueffel,eValue);
return 1;
}
Reference in New Issue View Git Blame Copy Permalink