Initial revision
This commit is contained in:
cvs
506
maximize.c
Normal file
506
maximize.c
Normal file
@ -0,0 +1,506 @@
|
||||
/*-----------------------------------------------------------------------
|
||||
M A X I M I Z E
|
||||
|
||||
This optimises a peak against a motor or virtual motor. The algorithm
|
||||
used is the same as in the program difrac by P. White and E. Gabe. The
|
||||
algorithm used is more efficient then the one based on scans as in
|
||||
fitcenter and optimise as it does not need as many counting operations.
|
||||
|
||||
The algorithm works as follows:
|
||||
|
||||
Data is kept in an array of length MAXPTS. We start at the middle of
|
||||
the array.
|
||||
|
||||
- From the current position we first step to the left side until we reach
|
||||
a point where the counts are less then half maximum or to many points.
|
||||
Now there are four possible situations:
|
||||
|
||||
- The peak is still to our left. Reposition and do again.
|
||||
- No peak found or peak is normal, i.e max close to 50. In either case
|
||||
proceed with right side.
|
||||
- peak all on left side. Calculate some indexes and calculate meridian.
|
||||
|
||||
- From the current position go to the right side until we reach the
|
||||
point where counts are less then half maximum or the maximum number
|
||||
of pints has been reached. Again four cases:
|
||||
|
||||
- The peak is still to our right. Reposition and do again.
|
||||
- No peak found: error return.
|
||||
- Normal peak: calculate meridian.
|
||||
- peak all on left side. Calculate some indexes and calculate meridian.
|
||||
|
||||
- Calculate the median of the peak.
|
||||
|
||||
|
||||
copyright: see copyright.h
|
||||
|
||||
|
||||
Initial coding: Mark Koennecke, November 1999
|
||||
-------------------------------------------------------------------------*/
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <tcl.h>
|
||||
#include "fortify.h"
|
||||
#include "sics.h"
|
||||
#include "countdriv.h"
|
||||
#include "counter.h"
|
||||
#include "drive.h"
|
||||
#include "maximize.h"
|
||||
|
||||
#define MAXPTS 100
|
||||
#define DEBUG 1
|
||||
|
||||
typedef struct __MAXIMIZE {
|
||||
pObjectDescriptor pDes;
|
||||
pCounter pCount;
|
||||
int i360;
|
||||
}Maxxii;
|
||||
|
||||
/*-----------------------------------------------------------------------
|
||||
put into 360 degree range if 360 degree flag is active.
|
||||
*/
|
||||
static float in360(pMax self, float fVal)
|
||||
{
|
||||
if(self->i360)
|
||||
{
|
||||
if(fVal < 0.)
|
||||
fVal = 360. + fVal;
|
||||
if(fVal > 360.)
|
||||
fVal = fVal - 360.;
|
||||
}
|
||||
return fVal;
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int maxDrive(void *pObject, char *pVarName,
|
||||
float fPos, SConnection *pCon)
|
||||
{
|
||||
pDummy pDum;
|
||||
int status;
|
||||
char pBueffel[132];
|
||||
long lTask;
|
||||
|
||||
/* start */
|
||||
pDum = (pDummy)pObject;
|
||||
status = StartDevice(pServ->pExecutor,
|
||||
pVarName,
|
||||
pDum->pDescriptor,
|
||||
pObject,
|
||||
pCon,
|
||||
fPos);
|
||||
if(!status)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: failed to start %s",pVarName);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* wait */
|
||||
lTask = GetDevexecID(pServ->pExecutor);
|
||||
if(lTask > 0)
|
||||
{
|
||||
TaskWait(pServ->pTasker,lTask);
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* check interrupts */
|
||||
if(SCGetInterrupt(pCon) >= eAbortScan)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: Maximizing interrupted",eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int maxCount(void *pCount, CounterMode eMode,
|
||||
float fPreset, long *lCts, SConnection *pCon)
|
||||
{
|
||||
int iRet;
|
||||
|
||||
SetCounterMode((pCounter)pCount,eMode);
|
||||
iRet = DoCount((pCounter)pCount, fPreset, pCon,1);
|
||||
if(!iRet)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
*lCts = GetCounts((pCounter)pCount,pCon);
|
||||
if(SCGetInterrupt(pCon) >= eAbortScan)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: maximizing aborted",eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
int MaximizePeak(pMax self, void *pVar, char *pVarName,
|
||||
float fStep, CounterMode eMode,
|
||||
float fPreset, SConnection *pCon)
|
||||
{
|
||||
float x[MAXPTS], y[MAXPTS];
|
||||
float fMax, fCent, fS, fArea, fStart, fPos, fDA, fDisc;
|
||||
int iBot, iTop, iMax, i, iRet, iSkip, iTMin, iTMax;
|
||||
pIDrivable pDriv = NULL;
|
||||
long lCts, lMax, lMin;
|
||||
char pBueffel[132];
|
||||
|
||||
/* get drivable interface */
|
||||
pDriv = GetDrivableInterface(pVar);
|
||||
if(!pDriv)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: variable is NOT drivable, cannot maximize",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
start:
|
||||
lMax = 0;
|
||||
lMin = 999999999999;
|
||||
fStart = pDriv->GetValue(pVar,pCon);
|
||||
if(fStart < -999999.)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* search to the left until out of space or lCts < lMax/2. */
|
||||
SCWrite(pCon,"Searching for low angle boundary..",eWarning);
|
||||
for(i = MAXPTS/2; i >= 0; i--)
|
||||
{
|
||||
/* drive motor */
|
||||
fPos = fStart - (MAXPTS/2 - i)*fStep;
|
||||
fPos = in360(self,fPos);
|
||||
if(maxDrive(pVar,pVarName,fPos,pCon) != 1)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
x[i] = pDriv->GetValue(pVar,pCon);
|
||||
/* count */
|
||||
if(maxCount(self->pCount,eMode,fPreset, &lCts,pCon) != 1)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
/* print a message */
|
||||
sprintf(pBueffel,"%5d %8.2f %ld",i,x[i],lCts);
|
||||
SCWrite(pCon,pBueffel,eWarning);
|
||||
|
||||
/* store counts and some logic */
|
||||
if(lCts < 1)
|
||||
{
|
||||
y[i] = 0.;
|
||||
}
|
||||
else
|
||||
{
|
||||
y[i] = (float)lCts;
|
||||
}
|
||||
if(lCts > lMax)
|
||||
{
|
||||
lMax = lCts;
|
||||
iMax = i;
|
||||
}
|
||||
if(lCts < lMin)
|
||||
{
|
||||
lMin = lCts;
|
||||
}
|
||||
if(lCts < lMax/2)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
/* some logic: the four cases */
|
||||
iSkip = 0;
|
||||
iBot = i;
|
||||
/* first case: peak out of range: do again */
|
||||
if( (iMax == 0) && (lMax/2 > lMin) )
|
||||
{
|
||||
goto start;
|
||||
}
|
||||
/* no peak found or normal peak: continue at other side */
|
||||
if( (i < 1) || (y[MAXPTS/2] > lMax/2) )
|
||||
{
|
||||
iSkip = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* next case: all of the peak in measured half:
|
||||
find max value and skip the right half
|
||||
*/
|
||||
for(i = MAXPTS/2; i > 0; i--)
|
||||
{
|
||||
if(y[i] > lMax/2)
|
||||
{
|
||||
iTop = i + 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
iSkip = 1;
|
||||
}
|
||||
|
||||
/*
|
||||
do the same to the right hand side, but only if required
|
||||
*/
|
||||
if(iSkip == 0)
|
||||
{
|
||||
lMin = 100000;
|
||||
lMax = -100000;
|
||||
SCWrite(pCon,"Searching for high angle boundary..",eWarning);
|
||||
for(i = MAXPTS/2; i < MAXPTS; i++)
|
||||
{
|
||||
/* drive motor */
|
||||
fPos = fStart + (i - MAXPTS/2) * fStep;
|
||||
fPos = in360(self,fPos);
|
||||
if(maxDrive(pVar,pVarName,fPos,pCon) != 1)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
x[i] = pDriv->GetValue(pVar,pCon);
|
||||
/* count */
|
||||
if(maxCount(self->pCount,eMode,fPreset, &lCts,pCon) != 1)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
/* print a message */
|
||||
sprintf(pBueffel,"%5d %8.2f %ld",i,x[i],lCts);
|
||||
SCWrite(pCon,pBueffel,eWarning);
|
||||
|
||||
/* store counts and some logic */
|
||||
if(lCts < 1)
|
||||
{
|
||||
y[i] = 0.;
|
||||
}
|
||||
else
|
||||
{
|
||||
y[i] = (float)lCts;
|
||||
}
|
||||
if(lCts > lMax)
|
||||
{
|
||||
lMax = lCts;
|
||||
iMax = i;
|
||||
}
|
||||
if(lCts < lMin)
|
||||
{
|
||||
lMin = lCts;
|
||||
}
|
||||
if(lCts < lMax/2)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
/* some logic again: four cases */
|
||||
iTop = i;
|
||||
iTop++;
|
||||
/* first case: peak is at high angle side */
|
||||
if( (i > MAXPTS-2) && (lMax*0.5 > lMin) )
|
||||
{
|
||||
goto start;
|
||||
}
|
||||
/* second case: no peak */
|
||||
if( (iTop > MAXPTS-2) )
|
||||
{
|
||||
SCWrite(pCon,"ERROR: no peak found!",eError);
|
||||
return 0;
|
||||
}
|
||||
/* third case: normal peak */
|
||||
if(y[MAXPTS/2] >= 0.5*lMax)
|
||||
{
|
||||
iTop--;
|
||||
}
|
||||
/*
|
||||
fourth case: the peak is completely at the high angle
|
||||
side
|
||||
*/
|
||||
else
|
||||
{
|
||||
for(i = MAXPTS/2; i < MAXPTS; i++)
|
||||
{
|
||||
if(y[i] > lMax/2)
|
||||
{
|
||||
iBot = i - 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
iTop--;
|
||||
}
|
||||
} /* end of iSkip */
|
||||
|
||||
if( (iBot < 2) || (iTop > MAXPTS-2) || (lMax < lMin*2) )
|
||||
{
|
||||
SCWrite(pCon,"ERROR: no peak found!",eError);
|
||||
return 0;
|
||||
}
|
||||
#ifdef DEBUG
|
||||
sprintf(pBueffel,"iBot = %d, iTop = %d, lMax = %d",
|
||||
iBot, iTop, lMax);
|
||||
SCWrite(pCon,pBueffel,eWarning);
|
||||
#endif
|
||||
|
||||
/* calculate median */
|
||||
fArea = 0.;
|
||||
for(i = iBot; i < iTop; i++)
|
||||
{
|
||||
fArea += (x[i+1] - x[i])*(y[i+1] + y[i])*.25;
|
||||
}
|
||||
fS = 0.;
|
||||
for(i = iBot; i < iTop; i++)
|
||||
{
|
||||
fS += (x[i+1] -x[i])*(y[i+1] +y[i]) *.5;
|
||||
if(fS > fArea)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
fS -= .5*(x[i+1] - x[i])*(y[i+1] + y[i]);
|
||||
fDA = fArea - fS;
|
||||
if(y[i+1] == y[i])
|
||||
{
|
||||
fCent = x[i] + fDA/y[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
fS = (y[i+1] - y[i])/(x[i+1] -x[i]);
|
||||
if( (y[i]*y[i] + 2. * fS * fDA) < 0.)
|
||||
{
|
||||
SCWrite(pCon,"ERROR calculating median",eError);
|
||||
return 0;
|
||||
}
|
||||
fDisc = sqrt(y[i]*y[i] + 2.* fS * fDA);
|
||||
fCent = x[i] +(fDisc -y[i])/fS;
|
||||
}
|
||||
/* finished ! */
|
||||
maxDrive(pVar,pVarName,fCent,pCon);
|
||||
sprintf(pBueffel,"Found peak center at %8.2f", fCent);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
static void MaxKill(void *pData)
|
||||
{
|
||||
pMax self = (pMax)pData;
|
||||
if(self == NULL)
|
||||
return;
|
||||
|
||||
if(self->pDes)
|
||||
DeleteDescriptor(self->pDes);
|
||||
free(self);
|
||||
}
|
||||
/*-------------------- wrap-i-wrap ----------------------------------*/
|
||||
int MaximizeFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pMax pNew = NULL;
|
||||
CommandList *pCom = NULL;
|
||||
pICountable pCts = NULL;
|
||||
|
||||
if(argc < 2)
|
||||
{
|
||||
SCWrite(pCon,
|
||||
"ERROR: insufficient number of arguments to MaximizeFactory",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* find a countable */
|
||||
pCom = FindCommand(pSics,argv[1]);
|
||||
if(pCom == NULL)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: counter not found in MaximizeFactory",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
pCts = GetCountableInterface(pCom->pData);
|
||||
if(!pCts)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: argument to MaximizeFactory is no counter",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
pNew = (pMax)malloc(sizeof(Maxxii));
|
||||
pNew->pDes = CreateDescriptor("Maximizer");
|
||||
pNew->pCount = pCom->pData;
|
||||
pNew->i360 = 0;
|
||||
|
||||
AddCommand(pSics,"max",MaximizeAction,MaxKill,pNew);
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
int MaximizeAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pMax self = NULL;
|
||||
CommandList *pCom = NULL;
|
||||
char pBueffel[256];
|
||||
double dVal;
|
||||
float fStep, fPreset;
|
||||
CounterMode eCount;
|
||||
int iRet;
|
||||
|
||||
self = (pMax)pData;
|
||||
assert(self);
|
||||
assert(pCon);
|
||||
|
||||
/* check privilege */
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* enough arguments ?*/
|
||||
if(argc < 5)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: Insufficient number of arguments to max",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* decode arguments */
|
||||
pCom = FindCommand(pSics,argv[1]);
|
||||
if(!pCom)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: object %s NOT found!",
|
||||
argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
if(!pCom->pData)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: object %s Invalid!!",
|
||||
argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
iRet = Tcl_GetDouble(pSics->pTcl,argv[2], &dVal);
|
||||
if(iRet != TCL_OK)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: failed to convert %s to number",
|
||||
argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
fStep = (float)dVal;
|
||||
|
||||
iRet = Tcl_GetDouble(pSics->pTcl,argv[4], &dVal);
|
||||
if(iRet != TCL_OK)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: failed to convert %s to number",
|
||||
argv[4]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
fPreset = (float)dVal;
|
||||
|
||||
/* convert countmode */
|
||||
strtolower(argv[3]);
|
||||
if(strcmp(argv[3],"timer") == 0)
|
||||
{
|
||||
eCount = eTimer;
|
||||
}
|
||||
else
|
||||
{
|
||||
eCount = ePreset;
|
||||
}
|
||||
return MaximizePeak(self,pCom->pData, argv[1],fStep,eCount,
|
||||
fPreset, pCon);
|
||||
}
|
||||
Reference in New Issue
Block a user