linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
3cb901b43794d697485a289f935eb2d0be461af2
sics/tasdrive.c
koennecke 9737722b8c - Added hmslave.c 
- Tasub only drives required motors
- Tasub handles fixed motors more gracefully
2007-05-30 11:57:50 +00:00

753 lines
21 KiB
C
Raw Blame History

/*---------------------------------------------------------------------
SICS interface to the triple axis drivable motors.
copyright: see file COPYRIGHT
Mark Koennecke, May 2005
In the long run it might be useful to switch all this to the more
general motor list driving code.
--------------------------------------------------------------------*/
#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);
if(self->code > 5 && self->math->tasMode == ELASTIC){
SCWrite(pCon,"ERROR: cannot drive this motor in elastic mode",
eError);
return HWFault;
}
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);
if(self->code > 5 && self->math->tasMode == ELASTIC){
SCWrite(pCon,"WARNING: value for this motor is meaningless in elastic mode",
eWarning);
}
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 float getMotorValue(pMotor mot, SConnection *pCon){
float val;
MotorGetSoftPosition(mot,pCon,&val);
return val;
}
/*--------------------------------------------------------------------------*/
static int startTASMotor(pMotor mot, SConnection *pCon, char *name,
double target, int silent){
float val, fixed;
int status = OKOK;
char buffer[132];
val = getMotorValue(mot,pCon);
MotorGetPar(mot,"fixed",&fixed);
if(ABS(fixed - 1.0) < .1) {
snprintf(buffer,131,"WARNING: %s is FIXED", name);
SCWrite(pCon,buffer,eWarning);
return OKOK;
}
if(ABS(val - target) > MOTPREC){
status = mot->pDrivInt->SetValue(mot,pCon,(float)target);
if(status != OKOK){
return status;
}
}
writeMotPos(pCon,silent,name,val, target);
return status;
}
/*---------------------------------------------------------------------------*/
static int startMotors(ptasMot self, tasAngles angles,
SConnection *pCon, int driveQ, int driveTilt){
double curve;
int status, silent;
silent = self->math->silent;
self->math->mustRecalculate = 1;
/*
monochromator
*/
status = startTASMotor(self->math->motors[A1],pCon, "a1",
angles.monochromator_two_theta/2.,silent);
if(status != OKOK){
return status;
}
status = startTASMotor(self->math->motors[A2],pCon, "a2",
angles.monochromator_two_theta,silent);
if(status != OKOK){
return status;
}
if(self->math->motors[MCV] != NULL){
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta);
status = startTASMotor(self->math->motors[MCV],pCon, "mcv",
curve,silent);
if(status != OKOK){
return status;
}
}
if(self->math->motors[MCH] != NULL){
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta);
status = startTASMotor(self->math->motors[MCH],pCon, "mch",
curve,silent);
if(status != OKOK){
return status;
}
}
/*
analyzer
*/
if(self->math->tasMode != ELASTIC){
status = startTASMotor(self->math->motors[A5],pCon, "a5",
angles.analyzer_two_theta/2.0,silent);
if(status != OKOK){
return status;
}
status = startTASMotor(self->math->motors[A6],pCon, "a6",
angles.analyzer_two_theta,silent);
if(status != OKOK){
return status;
}
if(self->math->motors[ACV] != NULL){
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta);
status = startTASMotor(self->math->motors[ACV],pCon, "acv",
curve,silent);
if(status != OKOK){
return status;
}
}
if(self->math->motors[ACH] != NULL){
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta);
status = startTASMotor(self->math->motors[ACH],pCon, "ach",
curve,silent);
if(status != OKOK){
return status;
}
}
}
if(driveQ == 0){
return OKOK;
}
/*
crystal
*/
status = startTASMotor(self->math->motors[A3],pCon, "a3",
angles.a3,silent);
if(status != OKOK){
return status;
}
status = startTASMotor(self->math->motors[A4],pCon, "a4",
angles.sample_two_theta,silent);
if(status != OKOK){
return status;
}
if(driveTilt == 1){
status = startTASMotor(self->math->motors[SGL],pCon, "sgl",
angles.sgl,silent);
if(status != OKOK){
return status;
}
status = startTASMotor(self->math->motors[SGU],pCon, "sgu",
angles.sgu,silent);
if(status != OKOK){
return status;
}
}
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];
float val;
MotorGetPar(self->math->motors[A3],"fixed",&val);
if((int)val != 1){
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;
int mask[12];
self->math->mustRecalculate = 1;
if(self->math->tasMode == ELASTIC){
length = 8;
}
memset(mask,0,12*sizeof(int));
for(i = 0; i < length; i++){
mask[i] = 1;
}
if(self->math->outOfPlaneAllowed != 0){
mask[SGU] = 0;
mask[SGL] = 0;
}
for(i = 0; i < 12; i++){
if(self->math->motors[i] != NULL && mask[i] != 0){
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, silent;
silent = self->math->silent;
/*
monochromator
*/
status = startTASMotor(self->math->motors[A1],pCon, "a1",
angles.monochromator_two_theta/2.,silent);
if(status != OKOK){
return status;
}
status = startTASMotor(self->math->motors[A2],pCon, "a2",
angles.monochromator_two_theta,silent);
if(status != OKOK){
return status;
}
if(self->math->motors[MCV] != NULL){
curve = maCalcVerticalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta);
status = startTASMotor(self->math->motors[MCV],pCon, "mcv",
curve,silent);
if(status != OKOK){
return status;
}
}
if(self->math->motors[MCH] != NULL){
curve = maCalcHorizontalCurvature(self->math->machine.monochromator,
angles.monochromator_two_theta);
status = startTASMotor(self->math->motors[MCH],pCon, "mch",
curve,silent);
if(status != OKOK){
return status;
}
}
/*
analyzer
*/
status = startTASMotor(self->math->motors[A5],pCon, "a5",
angles.analyzer_two_theta/2.0,silent);
if(status != OKOK){
return status;
}
status = startTASMotor(self->math->motors[A6],pCon, "a6",
angles.analyzer_two_theta,silent);
if(status != OKOK){
return status;
}
if(self->math->motors[ACV] != NULL){
curve = maCalcVerticalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta);
status = startTASMotor(self->math->motors[ACV],pCon, "acv",
curve,silent);
if(status != OKOK){
return status;
}
}
if(self->math->motors[ACH] != NULL){
curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
angles.analyzer_two_theta);
status = startTASMotor(self->math->motors[ACH],pCon, "ach",
curve,silent);
if(status != OKOK){
return status;
}
}
/*
crystal
*/
status = startTASMotor(self->math->motors[A4],pCon, "a4",
angles.sample_two_theta,silent);
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