- Arrayutil was not summing correctly - Added interest to motorsec in order to support old status clients - core dump because of bad free in devser - fixed transferring of count time in multicounter - added stack debugging to sllinux_def - Modifications to tasdrive to wait for finish of monochromator properly
857 lines
24 KiB
C
857 lines
24 KiB
C
/*---------------------------------------------------------------------
|
|
SICS interface to the triple axis drivable motors.
|
|
|
|
copyright: see file COPYRIGHT
|
|
|
|
Mark Koennecke, May 2005
|
|
|
|
|
|
Modified to rather use drivables then motors
|
|
|
|
Mark Koennecke, September 2011
|
|
|
|
Modified to use drive tasks
|
|
|
|
Mark Koennecke, January 2014
|
|
--------------------------------------------------------------------*/
|
|
#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);
|
|
int qcodes[] = {3,4,5,8}; /* qh,qk,ql, en */
|
|
int i;
|
|
|
|
if (self->code > 5 && self->math->tasMode == ELASTIC) {
|
|
SCWrite(pCon, "ERROR: cannot drive this motor in elastic mode",
|
|
eLogError);
|
|
return HWFault;
|
|
}
|
|
setTasPar(&self->math->target, self->math->tasMode, self->code, value);
|
|
self->math->mustDrive = 1;
|
|
for(i = 0; i < 4; i++){
|
|
if(self->code == qcodes[i]) {
|
|
self->math->mustDriveQ = 1;
|
|
}
|
|
}
|
|
return OKOK;
|
|
}
|
|
|
|
/*----------------------------------------------------------------*/
|
|
int readTASMotAngles(ptasUB self, SConnection * pCon,
|
|
ptasAngles ang)
|
|
{
|
|
int status;
|
|
float val, theta;
|
|
|
|
/*
|
|
Monochromator
|
|
*/
|
|
status = GetDrivablePosition(self->motors[A1], pCon, &val);
|
|
if (status == 0) {
|
|
return status;
|
|
}
|
|
theta = val;/* $Id: tasdrive.c,v 1.39 2014/02/18 13:25:32 koennecke Exp $ */
|
|
|
|
#ifndef lint
|
|
static char vcid[] = "$Id: tasdrive.c,v 1.39 2014/02/18 13:25:32 koennecke Exp $";
|
|
#endif /* lint */
|
|
|
|
status = GetDrivablePosition(self->motors[A2], pCon, &val);
|
|
if (status == 0) {
|
|
return status;
|
|
}
|
|
ang->monochromator_two_theta = val;
|
|
if (ABS(val / 2. - theta) > .1) {
|
|
if(DevExecLevelRunning(pServ->pExecutor,RUNRUN) != 1) {
|
|
SCWrite(pCon, "WARNING: theta monochromator not half of two theta",
|
|
eWarning);
|
|
}
|
|
}
|
|
|
|
/*
|
|
Analyzer
|
|
*/
|
|
if (self->tasMode != ELASTIC) {
|
|
status = GetDrivablePosition(self->motors[A5], pCon, &val);
|
|
if (status == 0) {
|
|
return status;
|
|
}
|
|
theta = val;
|
|
status = GetDrivablePosition(self->motors[A6], pCon, &val);
|
|
if (status == 0) {
|
|
return status;
|
|
}
|
|
ang->analyzer_two_theta = val;
|
|
if (ABS(val / 2. - theta) > .1 && DevExecLevelRunning(pServ->pExecutor,RUNRUN) != 1) {
|
|
SCWrite(pCon, "WARNING: theta analyzer not half of two theta",
|
|
eWarning);
|
|
}
|
|
} else {
|
|
ang->analyzer_two_theta = ang->monochromator_two_theta;
|
|
}
|
|
|
|
/*
|
|
crystal
|
|
*/
|
|
status = GetDrivablePosition(self->motors[A3], pCon, &val);
|
|
if (status == 0) {
|
|
return status;
|
|
}
|
|
ang->a3 = val;
|
|
status = GetDrivablePosition(self->motors[A4], pCon, &val);
|
|
if (status == 0) {
|
|
return status;
|
|
}
|
|
ang->sample_two_theta = val;
|
|
status = GetDrivablePosition(self->motors[SGU], pCon, &val);
|
|
if (status == 0) {
|
|
return status;
|
|
}
|
|
ang->sgu = val;
|
|
status = GetDrivablePosition(self->motors[SGL], pCon, &val);
|
|
if (status == 0) {
|
|
return status;
|
|
}
|
|
ang->sgl = val;
|
|
if(!self->outOfPlaneAllowed){
|
|
ang->sgu = .0;
|
|
ang->sgl = .0;
|
|
}
|
|
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",
|
|
eLogError);
|
|
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",
|
|
eLogError);
|
|
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;
|
|
pIDrivable pDriv = NULL;
|
|
|
|
assert(self);
|
|
/**
|
|
* disregard non existing analyzer motors
|
|
*/
|
|
if (self->math->tasMode == ELASTIC) {
|
|
length = 8;
|
|
}
|
|
|
|
/*
|
|
stop monochromator
|
|
*/
|
|
self->math->mono->Halt(self->math->monoData);
|
|
|
|
/*
|
|
stop rest
|
|
*/
|
|
for (i = 4; i < length; i++) {
|
|
if (self->math->motors[i] != NULL) {
|
|
pDriv = GetDrivableInterface(self->math->motors[i]);
|
|
pDriv->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, eLog);
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
static float getMotorValue(pMotor mot, SConnection * pCon)
|
|
{
|
|
float val;
|
|
|
|
GetDrivablePosition(mot, pCon, &val);
|
|
return val;
|
|
}
|
|
/*--------------------- In devexec.c --------------------------------------*/
|
|
void InvokeNewTarget(pExeList self, char *name, float target);
|
|
/*--------------------------------------------------------------------------*/
|
|
static int startTASMotor(pMotor mot, SConnection * pCon, char *name,
|
|
double target, int silent, int stopFixed, long groupID)
|
|
{
|
|
float val, fixed, precision = MOTPREC;
|
|
int status = OKOK;
|
|
char buffer[132];
|
|
pIDrivable pDriv = NULL;
|
|
pDummy dum = NULL;
|
|
pDynString mes = NULL;
|
|
long taskID;
|
|
|
|
dum = (pDummy)mot;
|
|
val = getMotorValue(mot, pCon);
|
|
if(strcmp(dum->pDescriptor->name,"Motor") == 0){
|
|
MotorGetPar(mot,"precision",&precision);
|
|
MotorGetPar(mot, "fixed", &fixed);
|
|
if (ABS(fixed - 1.0) < .1) {
|
|
if(stopFixed == 0){
|
|
snprintf(buffer, 131, "WARNING: %s is FIXED", name);
|
|
SCWrite(pCon, buffer, eLog);
|
|
}
|
|
return OKOK;
|
|
}
|
|
}
|
|
mot->stopped = 0;
|
|
if (ABS(val - target) > precision) {
|
|
taskID = StartDriveTask(mot,pCon,name,target);
|
|
if(taskID < 0){
|
|
SCPrintf(pCon,eLog,"ERROR: failed to drive %s to %f", name, target);
|
|
} else {
|
|
AddTaskToGroup(pServ->pTasker,taskID, groupID);
|
|
}
|
|
/*
|
|
to force updates on targets
|
|
*/
|
|
InvokeNewTarget(pServ->pExecutor, name, target);
|
|
writeMotPos(pCon, silent, name, val, target);
|
|
}
|
|
return status;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int StartTASA3(pMotor mot, SConnection *pCon, char *name,
|
|
double target, int silent, int stopFixed,
|
|
long groupID)
|
|
{
|
|
float val;
|
|
int status;
|
|
char buffer[80];
|
|
|
|
val = getMotorValue(mot,pCon);
|
|
if(ABS(target-val) > 190) {
|
|
SCPrintf(pCon,eLog,"WARNING: driving a3 more then 190 degree: %f, please confirm with y",
|
|
ABS(target-val));
|
|
status = SCPromptTMO(pCon,NULL,buffer,sizeof(buffer),120);
|
|
if(status == 1){
|
|
if(buffer[0] == 'y'){
|
|
return startTASMotor(mot,pCon,name,target,silent,stopFixed,groupID);
|
|
}
|
|
}
|
|
SCWrite(pCon,"ERROR: no or wrong reply, aborting scan",eLogError);
|
|
SCSetInterrupt(pCon,eAbortScan);
|
|
return HWFault;
|
|
} else {
|
|
return startTASMotor(mot,pCon,name,target,silent,stopFixed,groupID);
|
|
}
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
static int startMotors(ptasMot self, tasAngles angles,
|
|
SConnection * pCon, int driveQ, int driveTilt)
|
|
{
|
|
double curve;
|
|
int status, silent, stopFixed;
|
|
long monoID;
|
|
|
|
silent = self->math->silent;
|
|
stopFixed = self->math->stopFixed;
|
|
self->math->mustRecalculate = 1;
|
|
|
|
self->math->groupID = GetTaskGroupID(pServ->pTasker);
|
|
|
|
/*
|
|
monochromator
|
|
*/
|
|
status = self->math->mono->SetValue(self->math->monoData,
|
|
pCon,angles.monochromator_two_theta);
|
|
/*
|
|
The call to CheckStatus is necessary because the eiger monochromator may not
|
|
start until then. Deferred until all parameters are known.
|
|
*/
|
|
self->math->mono->CheckStatus(self->math->monoData,pCon);
|
|
if(status != OKOK){
|
|
return status;
|
|
} else {
|
|
AddTaskToGroup(pServ->pTasker, self->math->monoTaskID, self->math->groupID);
|
|
}
|
|
/* monoID = StartDriveTask(self->math->monoData, pCon,"mono", */
|
|
/* angles.monochromator_two_theta); */
|
|
/* self->math->mono->CheckStatus(self->math->monoData,pCon); */
|
|
/* if(monoID < 0){ */
|
|
/* SCWrite(pCon,"ERROR: failed to start monochromator",eLogError); */
|
|
/* } else { */
|
|
/* AddTaskToGroup(pServ->pTasker,monoID, self->math->groupID); */
|
|
/* } */
|
|
|
|
/*
|
|
analyzer
|
|
*/
|
|
if (self->math->tasMode != ELASTIC) {
|
|
status = startTASMotor(self->math->motors[A5], pCon, "a5",
|
|
angles.analyzer_two_theta / 2.0, silent,stopFixed,self->math->groupID);
|
|
if (status != OKOK) {
|
|
return status;
|
|
}
|
|
status = startTASMotor(self->math->motors[A6], pCon, "a6",
|
|
angles.analyzer_two_theta, silent,stopFixed,self->math->groupID);
|
|
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,stopFixed,self->math->groupID);
|
|
if (status != OKOK) {
|
|
SCWrite(pCon,"WARNING: analyzer vertical curvature motor failed to start", eLog);
|
|
SCSetInterrupt(pCon,eContinue);
|
|
}
|
|
}
|
|
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,stopFixed,self->math->groupID);
|
|
if (status != OKOK) {
|
|
SCWrite(pCon,"WARNING: analyzer horizontal curvature motor failed to start", eLog);
|
|
SCSetInterrupt(pCon,eContinue);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (driveQ == 0) {
|
|
return OKOK;
|
|
}
|
|
|
|
/*
|
|
crystal
|
|
*/
|
|
status = StartTASA3(self->math->motors[A3], pCon, "a3",
|
|
angles.a3, silent,stopFixed,self->math->groupID);
|
|
if (status != OKOK) {
|
|
return status;
|
|
}
|
|
status = startTASMotor(self->math->motors[A4], pCon, "a4",
|
|
angles.sample_two_theta, silent,stopFixed,self->math->groupID);
|
|
if (status != OKOK) {
|
|
return status;
|
|
}
|
|
|
|
if (driveTilt == 1) {
|
|
status = startTASMotor(self->math->motors[SGL], pCon, "sgl",
|
|
angles.sgl, silent,stopFixed, self->math->groupID);
|
|
if (status != OKOK) {
|
|
return status;
|
|
}
|
|
status = startTASMotor(self->math->motors[SGU], pCon, "sgu",
|
|
angles.sgu, silent,stopFixed,self->math->groupID);
|
|
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;
|
|
pIDrivable pDrivInt = NULL;
|
|
|
|
MotorGetPar(self->math->motors[A3], "fixed", &val);
|
|
if ((int) val != 1) {
|
|
pDrivInt = GetDrivableInterface(self->math->motors[A3]);
|
|
status = 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, eLogError);
|
|
}
|
|
}
|
|
|
|
pDrivInt = GetDrivableInterface(self->math->motors[A4]);
|
|
status = 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, eLogError);
|
|
}
|
|
|
|
if (driveTilt == 1) {
|
|
pDrivInt = GetDrivableInterface(self->math->motors[SGU]);
|
|
status = 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, eLogError);
|
|
}
|
|
|
|
pDrivInt = GetDrivableInterface(self->math->motors[SGL]);
|
|
status = 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, eLogError);
|
|
}
|
|
}
|
|
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", eLogError);
|
|
return HWFault;
|
|
break;
|
|
case UBNOMEMORY:
|
|
SCWrite(pCon, "ERROR: out of memory calculating angles", eLogError);
|
|
driveQ = 0;
|
|
break;
|
|
case BADRMATRIX:
|
|
SCWrite(pCon, "ERROR: bad crystallographic parameters or bad UB",
|
|
eLogError);
|
|
driveQ = 0;
|
|
break;
|
|
case BADUBORQ:
|
|
SCWrite(pCon, "ERROR: bad UB matrix or bad Q-vector", eLogError);
|
|
driveQ = 0;
|
|
break;
|
|
case TRIANGLENOTCLOSED:
|
|
SCWrite(pCon, "ERROR: cannot close scattering triangle", eLogError);
|
|
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);
|
|
}
|
|
mat_free(scatteringPlaneNormal);
|
|
}
|
|
driveTilt = 0;
|
|
}
|
|
|
|
if(self->math->mustDriveQ == 0){
|
|
driveQ = 0;
|
|
}
|
|
|
|
|
|
if (driveQ != 0) {
|
|
if (!checkQMotorLimits(self, pCon, angles, driveTilt)) {
|
|
driveQ = 0;
|
|
}
|
|
}
|
|
|
|
if (driveQ == 0 && self->math->mustDriveQ == 1) {
|
|
SCWrite(pCon, "WARNING: NOT driving Q-vector because of errors",
|
|
eLogError);
|
|
}
|
|
/*
|
|
* reset the Q flag
|
|
*/
|
|
self->math->mustDriveQ = 0;
|
|
return startMotors(self, angles, pCon, driveQ, driveTilt);
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------------*/
|
|
static int checkMotors(ptasMot self, SConnection * pCon)
|
|
{
|
|
self->math->mustRecalculate = 1;
|
|
|
|
if(isTaskGroupRunning(pServ->pTasker,self->math->groupID) ||
|
|
isTaskGroupRunning(pServ->pTasker,self->math->monoTaskID)){
|
|
return HWBusy;
|
|
} else {
|
|
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) {
|
|
TASHalt(pData);
|
|
return HWBusy;
|
|
} else {
|
|
return HWBusy;
|
|
}
|
|
} else {
|
|
return checkMotors(self, pCon);
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
static int startQMMotors(ptasMot self, tasAngles angles,
|
|
SConnection * pCon)
|
|
{
|
|
float val;
|
|
double curve;
|
|
int status, silent, stopFixed;
|
|
|
|
silent = self->math->silent;
|
|
stopFixed = self->math->stopFixed;
|
|
|
|
self->math->groupID = GetTaskGroupID(pServ->pTasker);
|
|
|
|
/*
|
|
monochromator
|
|
*/
|
|
status = self->math->mono->SetValue(self->math->monoData,
|
|
pCon,angles.monochromator_two_theta);
|
|
if(status != OKOK){
|
|
return status;
|
|
} else {
|
|
AddTaskToGroup(pServ->pTasker, self->math->monoTaskID, self->math->groupID);
|
|
}
|
|
|
|
|
|
/*
|
|
analyzer
|
|
*/
|
|
status = startTASMotor(self->math->motors[A5], pCon, "a5",
|
|
angles.analyzer_two_theta / 2.0, silent,stopFixed,self->math->groupID);
|
|
if (status != OKOK) {
|
|
return status;
|
|
}
|
|
status = startTASMotor(self->math->motors[A6], pCon, "a6",
|
|
angles.analyzer_two_theta, silent,stopFixed,self->math->groupID);
|
|
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,stopFixed,self->math->groupID);
|
|
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,stopFixed,self->math->groupID);
|
|
if (status != OKOK) {
|
|
return status;
|
|
}
|
|
}
|
|
|
|
/*
|
|
crystal
|
|
*/
|
|
status = startTASMotor(self->math->motors[A4], pCon, "a4",
|
|
angles.sample_two_theta, silent,stopFixed,self->math->groupID);
|
|
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", eLogError);
|
|
return HWFault;
|
|
break;
|
|
case TRIANGLENOTCLOSED:
|
|
SCWrite(pCon, "ERROR: cannot close scattering triangle", eLogError);
|
|
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;
|
|
}
|