28bee49727
- Changed floor() to round() in sanslirebin - Some changes to accomodate the new run/drive behaviour - Added the the sps bipa command
295 lines
8.5 KiB
C
295 lines
8.5 KiB
C
/*--------------------------------------------------------------------------
|
|
This is one implementation file for the TASMAD simulation module for
|
|
SICS. The requirement is to make SICS look as much as TASMAD as
|
|
possible. This includes:
|
|
- TASMAD is variable driven
|
|
- Sometimes variables are accessed in storage order.
|
|
- A complicated calculation has to be done for getting the instruments
|
|
settings right. The appropriate F77 routine from TASMAD will be
|
|
reused.
|
|
- The scan logic is different.
|
|
- Output of ILL-formatted data files is required.
|
|
|
|
This file implements the MAD dr command for driving.
|
|
|
|
Mark Koennecke, November 2000
|
|
|
|
Polarisation support added.
|
|
|
|
Mark Koennecke, April 2002
|
|
---------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <fortify.h>
|
|
#include <sics.h>
|
|
#include <sicsvar.h>
|
|
#include <counter.h>
|
|
#include <motor.h>
|
|
#include <scan.h>
|
|
#include <splitter.h>
|
|
#include "tas.h"
|
|
#include "tasu.h"
|
|
|
|
|
|
/* a token break function, implemented in stptok.c */
|
|
extern char *stptok(const char *s, char *tok, size_t toklen, char *brk);
|
|
|
|
#define VAR 1
|
|
#define VALUE 2
|
|
|
|
/*----------------------------------------------------------------------
|
|
TASDrive has to do an interesting parsing job: The normal syntax is
|
|
par=val. However, it is possible that the we get par = val, val, val
|
|
which means that the motors following par in storage order are
|
|
driven. Additionally we have to check for special energy or Q variables
|
|
which require a triple axis calculation.
|
|
|
|
M.Z. June 07: storage order mode only allowed for QH,QK,QL,EN.
|
|
|
|
It helps if one understands some fundamental things used in the code
|
|
below:
|
|
- motorMask holds a value for each motor in the motor list. The
|
|
value can be either 0 for not to drive or 1 for drive. After
|
|
successfull parsing these motors will be started. The mask will be built
|
|
during parsing.
|
|
- newPositions holds the new positions for the normal motors.
|
|
- tasMask is a mask which indicates which triple axis special variable
|
|
(Energy or Q) is driven.
|
|
- tasTargetMask will be set by the TAS calculation and will indicate
|
|
which motors of the range A1-A6, curvature and currents need to be
|
|
driven.
|
|
- tasTargets holds after the TAS calculation the target values for the
|
|
A1-A6, curvature and currents motors.
|
|
|
|
|
|
-------------------------------------------------------------------------*/
|
|
#define NUM 1
|
|
#define TXT 2
|
|
|
|
int TASDrive(SConnection * pCon, SicsInterp * pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pTASdata self = NULL;
|
|
int iTAS = 0;
|
|
float tasTargets[20], oldPos, oldEnergy[MAXEVAR];
|
|
unsigned char tasTargetMask[20], tasMask[MAXEVAR];
|
|
char *pPtr, pToken[20], pLine[256];
|
|
int varPointer, i, motorPointer, status, rStatus, lastToken;
|
|
char pBueffel[256];
|
|
unsigned char motorMask[MAXMOT];
|
|
float newPositions[MAXMOT];
|
|
pMotor pMot;
|
|
int qhPars;
|
|
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
self = (pTASdata) pData;
|
|
assert(self);
|
|
|
|
/*
|
|
check authorization
|
|
*/
|
|
if (!SCMatchRights(pCon, usUser))
|
|
return 0;
|
|
|
|
|
|
/* Initialize */
|
|
Arg2Text(argc, argv, pLine, 255);
|
|
strtolower(pLine);
|
|
lastToken = NUM;
|
|
pPtr = pLine + strlen(argv[0]); /* step over command */
|
|
for (i = 0; i < 10; i++) {
|
|
tasMask[i] = 0;
|
|
motorMask[i] = 0;
|
|
motorMask[10 + i] = 0;
|
|
tasTargets[i] = .0;
|
|
tasTargets[i + 10] = .0;
|
|
oldEnergy[i] = .0;
|
|
}
|
|
tasMask[10] = 0;
|
|
tasMask[11] = 0;
|
|
|
|
for (i = 0; i < MAXMOT; i++) {
|
|
motorMask[i] = 0;
|
|
}
|
|
varPointer = -1;
|
|
motorPointer = -1;
|
|
rStatus = 1;
|
|
qhPars = 0;
|
|
|
|
/* parse loop */
|
|
while (pPtr != NULL) {
|
|
pPtr = stptok(pPtr, pToken, 20, " ,=");
|
|
if (strlen(pToken) < 1 || pPtr == NULL)
|
|
continue;
|
|
|
|
if (tasNumeric(pToken)) { /* numbers */
|
|
if (lastToken == NUM) {
|
|
/* handle storage order logic */
|
|
if (varPointer > -1 && qhPars > 1) {
|
|
varPointer++;
|
|
qhPars--;
|
|
} else {
|
|
sprintf(pBueffel, "ERROR: parse error at %s, %s",
|
|
pToken, "need parameter to drive");
|
|
SCWrite(pCon, pBueffel, eError);
|
|
return 0;
|
|
}
|
|
}
|
|
/* enter the parameter to drive into the appropriate mask */
|
|
if (motorPointer >= 0) {
|
|
motorMask[motorPointer] = 1;
|
|
newPositions[motorPointer] = atof(pToken);
|
|
} else if (varPointer >= 0) {
|
|
tasMask[varPointer] = 1;
|
|
oldEnergy[varPointer] = self->tasPar[EMIN + varPointer]->fVal;
|
|
self->tasPar[EMIN + varPointer]->fVal = atof(pToken);
|
|
self->tasPar[ETARGET + varPointer]->fVal = atof(pToken);
|
|
} else {
|
|
sprintf(pBueffel, "ERROR: parse error at %s, %s",
|
|
pToken, "need parameter to drive");
|
|
SCWrite(pCon, pBueffel, eError);
|
|
return 0;
|
|
}
|
|
lastToken = NUM;
|
|
} else { /* text tokens */
|
|
|
|
lastToken = TXT;
|
|
if ((status = isTASEnergy(pToken)) > -1) { /* Ei, KI, EF, KF, Q.... */
|
|
iTAS = 1;
|
|
motorPointer = -1;
|
|
varPointer = status;
|
|
if (EMIN + varPointer == QH) {
|
|
qhPars = 4;
|
|
}
|
|
} else if ((status = isTASMotor(pToken)) > -1) {
|
|
motorPointer = status;
|
|
varPointer = -1;
|
|
} else {
|
|
sprintf(pBueffel, "ERROR: cannot drive %s", pToken);
|
|
SCWrite(pCon, pBueffel, eError);
|
|
rStatus = 0;
|
|
break;
|
|
}
|
|
}
|
|
} /* end of parse loop */
|
|
|
|
if (rStatus != 1)
|
|
return rStatus;
|
|
|
|
/*
|
|
store SRO motor value
|
|
*/
|
|
getSRO(pCon, &self->oldSRO);
|
|
|
|
|
|
/* having done this, we can start the motors */
|
|
for (i = 0; i < MAXMOT; i++) {
|
|
if (motorMask[i] > 0) {
|
|
pMot = FindMotor(pSics, tasMotorOrder[i]);
|
|
if (pMot) {
|
|
MotorGetSoftPosition(pMot, pCon, &oldPos);
|
|
sprintf(pBueffel, "Driving %s from %f to %f",
|
|
tasMotorOrder[i], oldPos, newPositions[i]);
|
|
} else {
|
|
sprintf(pBueffel, "Driving %s to %f",
|
|
tasMotorOrder[i], newPositions[i]);
|
|
}
|
|
SCWrite(pCon, pBueffel, eWarning);
|
|
status = StartMotor(pServ->pExecutor, pSics, pCon,
|
|
tasMotorOrder[i], RUNDRIVE, newPositions[i]);
|
|
if (status == 0) {
|
|
/* error should already have been reported by StartMotor */
|
|
rStatus = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
if in TAS mode do the TAS calculation and start the appropriate
|
|
motors.
|
|
*/
|
|
if (iTAS > 0) {
|
|
if (qhPars == 3) {
|
|
SCWrite(pCon, "ERROR: QH cannot have 2 values", eError);
|
|
return 0;
|
|
}
|
|
|
|
status = TASCalc(self, pCon, tasMask, tasTargets, tasTargetMask);
|
|
if (status) {
|
|
/*
|
|
do output, first Q-E variables
|
|
*/
|
|
for (i = 0; i < 12; i++) {
|
|
if (tasMask[i]) {
|
|
sprintf(pBueffel, "Driving %s from %f to %f",
|
|
tasVariableOrder[EI + i], oldEnergy[i],
|
|
self->tasPar[EI + i]->fVal);
|
|
SCWrite(pCon, pBueffel, eWarning);
|
|
}
|
|
}
|
|
/*
|
|
more output: the motor positions
|
|
*/
|
|
for (i = 0; i < 9; i++) {
|
|
if (tasTargetMask[i]) {
|
|
pMot = FindMotor(pSics, tasMotorOrder[i]);
|
|
if (pMot) {
|
|
MotorGetSoftPosition(pMot, pCon, &oldPos);
|
|
} else {
|
|
oldPos = -9999.;
|
|
}
|
|
sprintf(pBueffel, "Driving %s from %f to %f", tasMotorOrder[i],
|
|
oldPos, tasTargets[i]);
|
|
SCWrite(pCon, pBueffel, eWarning);
|
|
}
|
|
}
|
|
/*
|
|
output for magnet currents
|
|
*/
|
|
for (i = 0; i < 8; i++) {
|
|
if (tasTargetMask[9 + i]) {
|
|
oldPos = readDrivable(tasMotorOrder[CURMOT + i], pCon);
|
|
sprintf(pBueffel, "Driving %s from %f to %f",
|
|
tasMotorOrder[CURMOT + i], oldPos, tasTargets[9 + i]);
|
|
SCWrite(pCon, pBueffel, eWarning);
|
|
}
|
|
}
|
|
|
|
status = TASStart(self, pCon, pSics, tasTargets, tasTargetMask);
|
|
if (status == 0) {
|
|
rStatus = 0;
|
|
}
|
|
} else {
|
|
rStatus = 0;
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
wait till we are finished
|
|
*/
|
|
status = Wait4Success(GetExecutor());
|
|
TASUpdate(self, pCon);
|
|
|
|
/*
|
|
handle interrupts
|
|
*/
|
|
if (status == DEVINT) {
|
|
if (SCGetInterrupt(pCon) == eAbortOperation) {
|
|
SCSetInterrupt(pCon, eContinue);
|
|
sprintf(pBueffel, "Driving aborted");
|
|
SCWrite(pCon, pBueffel, eError);
|
|
}
|
|
return 0;
|
|
} else if (status == DEVDONE) {
|
|
sprintf(pBueffel, "Driving done");
|
|
SCWrite(pCon, pBueffel, eValue);
|
|
} else {
|
|
sprintf(pBueffel, "Driving finished");
|
|
SCWrite(pCon, pBueffel, eValue);
|
|
}
|
|
return rStatus;
|
|
}
|