- Conescan working now
- Removed old NETReadTillterm
This commit is contained in:
koennecke
336
tasdriveo.c
Normal file
336
tasdriveo.c
Normal file
@ -0,0 +1,336 @@
|
|||||||
|
/*--------------------------------------------------------------------------
|
||||||
|
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.
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
/* 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(motorPointer > -1)
|
||||||
|
{
|
||||||
|
motorPointer++;
|
||||||
|
}
|
||||||
|
else if(varPointer > -1)
|
||||||
|
{
|
||||||
|
varPointer++;
|
||||||
|
}
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
oldPos = -9999.;
|
||||||
|
}
|
||||||
|
sprintf(pBueffel,"Driving %s from %f to %f",
|
||||||
|
tasMotorOrder[i],oldPos,newPositions[i]);
|
||||||
|
SCWrite(pCon,pBueffel,eWarning);
|
||||||
|
status = StartMotor(pServ->pExecutor,pSics,pCon,
|
||||||
|
tasMotorOrder[i],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)
|
||||||
|
{
|
||||||
|
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,eStatus);
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
else if(status == DEVDONE)
|
||||||
|
{
|
||||||
|
sprintf(pBueffel,"Driving done");
|
||||||
|
SCWrite(pCon,pBueffel,eStatus);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
sprintf(pBueffel,
|
||||||
|
"Driving finished");
|
||||||
|
SCWrite(pCon,pBueffel,eStatus);
|
||||||
|
}
|
||||||
|
return rStatus;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Reference in New Issue
Block a user