linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
sics/maximize.c
Koennecke Mark 66466c1c0f This is the first working version of the new logging system. Some work 
in fine tuning still needs to be done. But is reasonably OK now.
2016-02-11 13:40:31 +01:00

500 lines
13 KiB
C
Raw Permalink Blame History

/*-----------------------------------------------------------------------
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"
#include "motor.h"
#define MAXPTS 100
typedef struct __MAXIMIZE {
pObjectDescriptor pDes;
pCounter pCount;
int i360;
int maxpts;
} 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];
/* start */
pDum = (pDummy) pObject;
status = StartDevice(pServ->pExecutor,
pVarName, pDum->pDescriptor, pObject, pCon,
RUNDRIVE, fPos);
if (!status) {
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: failed to start %s", pVarName);
SCWrite(pCon, pBueffel, eError);
return 0;
}
/* wait */
while(DevExecLevelRunning(pServ->pExecutor,RUNDRIVE)){
TaskYield(pServ->pTasker);
}
/* 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;
}
/*----------------------------------------------------------------------*/
static float readMPDrivable(void *pVar, SConnection * pCon)
{
float value = -999.99;
pIDrivable pDriv = NULL;
pDummy pDum = (pDummy) pVar;
pDriv = GetDrivableInterface(pVar);
assert(pDriv != NULL);
value = pDriv->GetValue(pVar, pCon);
return value;
}
/*-----------------------------------------------------------------------*/
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 = 0x7fffffff;
fStart = readMPDrivable(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..", eLog);
for (i = self->maxpts / 2; i >= 0; i--) {
/* drive motor */
fPos = fStart - (self->maxpts / 2 - i) * fStep;
fPos = in360(self, fPos);
if (maxDrive(pVar, pVarName, fPos, pCon) != 1) {
return 0;
}
x[i] = readMPDrivable(pVar, pCon);
/* count */
if (maxCount(self->pCount, eMode, fPreset, &lCts, pCon) != 1) {
return 0;
}
/* print a message */
snprintf(pBueffel,sizeof(pBueffel)-1, "%5d %8.2f %ld", i, x[i], lCts);
SCWrite(pCon, pBueffel, eLog);
/* 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[self->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 = self->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..", eLog);
for (i = self->maxpts / 2; i < self->maxpts; i++) {
/* drive motor */
fPos = fStart + (i - self->maxpts / 2) * fStep;
fPos = in360(self, fPos);
if (maxDrive(pVar, pVarName, fPos, pCon) != 1) {
return 0;
}
x[i] = readMPDrivable(pVar, pCon);
/* count */
if (maxCount(self->pCount, eMode, fPreset, &lCts, pCon) != 1) {
return 0;
}
/* print a message */
snprintf(pBueffel,sizeof(pBueffel)-1, "%5d %8.2f %ld", i, x[i], lCts);
SCWrite(pCon, pBueffel, eLog);
/* 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 > self->maxpts - 2) && (lMax * 0.5 > lMin)) {
goto start;
}
/* second case: no peak */
if ((iTop > self->maxpts - 2)) {
SCWrite(pCon, "ERROR: no peak found!", eError);
return 0;
}
/* third case: normal peak */
if (y[self->maxpts / 2] >= 0.5 * lMax) {
iTop--;
}
/*
fourth case: the peak is completely at the high angle
side
*/
else {
for (i = self->maxpts / 2; i < self->maxpts; i++) {
if (y[i] > lMax / 2) {
iBot = i - 1;
break;
}
}
iTop--;
}
}
/* end of iSkip */
if ((iBot < 2) || (iTop > self->maxpts - 2) || (lMax < lMin * 2)) {
SCWrite(pCon, "ERROR: no peak found!", eError);
return 0;
}
#ifdef DEBUG
snprintf(pBueffel,sizeof(pBueffel)-1, "iBot = %d, iTop = %d, lMax = %ld", 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 ! */
for (i = 0; i < iTop - 1; i++) {
if (fCent >= x[i] && fCent < x[i + 1]) {
lCts = y[i];
break;
}
}
maxDrive(pVar, pVarName, fCent, pCon);
snprintf(pBueffel,sizeof(pBueffel)-1, "Found peak center at %8.2f, Count = %ld", fCent,
lCts);
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;
pNew->maxpts = 100;
AddCommand(pSics, "max", MaximizeAction, MaxKill, pNew);
return 1;
}
/*------------------------------------------------------------------
* max motor step preset mode
* ---------------------------------------------------------------------*/
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, iVal;
self = (pMax) pData;
assert(self);
assert(pCon);
/* check privilege */
if (!SCMatchRights(pCon, usUser)) {
return 1;
}
/* enough arguments ? */
if (argc < 5) {
if (argc > 1) {
strtolower(argv[1]);
if (strcmp(argv[1], "in360") == 0) {
if (argc > 2) {
iVal = atoi(argv[2]);
if (iVal != 0 && iVal != 1) {
SCWrite(pCon, "ERROR: only 0, 1 allowed for in360", eError);
return 0;
}
self->i360 = iVal;
SCSendOK(pCon);
return 1;
} else {
snprintf(pBueffel, 255, "max.in360 = %d", self->i360);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
}
if (strcmp(argv[1], "maxpts") == 0) {
if (argc > 2) {
iVal = atoi(argv[2]);
if (iVal < 10 || iVal > 100) {
SCWrite(pCon, "ERROR: maxpst must be between 10 and 100",
eError);
return 0;
}
self->maxpts = iVal;
SCSendOK(pCon);
return 1;
} else {
snprintf(pBueffel, 255, "max.maxpts = %d", self->maxpts);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
}
}
SCWrite(pCon, "ERROR: Insufficient number of arguments to max",
eError);
return 0;
}
/* decode arguments */
pCom = FindCommand(pSics, argv[1]);
if (!pCom) {
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: object %s NOT found!", argv[1]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
if (!pCom->pData) {
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: object %s Invalid!!", argv[1]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
iRet = Tcl_GetDouble(pSics->pTcl, argv[2], &dVal);
if (iRet != TCL_OK) {
snprintf(pBueffel,sizeof(pBueffel)-1, "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) {
snprintf(pBueffel,sizeof(pBueffel)-1, "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 View Git Blame Copy Permalink