linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

PSI update

Browse Source 
r1464 | ffr | 2007-02-12 12:20:21 +1100 (Mon, 12 Feb 2007) | 2 lines
This commit is contained in:
Ferdi Franceschini
2007-02-12 12:20:21 +11:00
committed by Douglas Clowes
parent 634f2023b1
commit 3168325921
157 changed files with 29053 additions and 910 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
maximize.c
View File

@@ -47,6 +47,7 @@
#include "counter.h"
#include "drive.h"
#include "maximize.h"
#include "motor.h"
#define MAXPTS 100
#define DEBUG 1
@@ -55,6 +56,7 @@
pObjectDescriptor pDes;
pCounter pCount;
int i360;
int maxpts;
}Maxxii;
/*-----------------------------------------------------------------------
@@ -134,7 +136,25 @@
}
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);
if(strcmp(pDum->pDescriptor->name,"Motor") == 0)
{
MotorGetSoftPosition((pMotor)pVar,pCon,&value);
}
else
{
value = pDriv->GetValue(pVar,pCon);
}
return value;
}
/*-----------------------------------------------------------------------*/
int MaximizePeak(pMax self, void *pVar, char *pVarName,
float fStep, CounterMode eMode,
@@ -159,27 +179,27 @@
start:
lMax = 0;
lMin = 0x7fffffff;
fStart = pDriv->GetValue(pVar,pCon);
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..",eWarning);
for(i = MAXPTS/2; i >= 0; i--)
for(i = self->maxpts/2; i >= 0; i--)
{
/* drive motor */
fPos = fStart - (MAXPTS/2 - i)*fStep;
fPos = fStart - (self->maxpts/2 - i)*fStep;
fPos = in360(self,fPos);
if(maxDrive(pVar,pVarName,fPos,pCon) != 1)
{
{
return 0;
}
x[i] = pDriv->GetValue(pVar,pCon);
x[i] = readMPDrivable(pVar,pCon);
/* count */
if(maxCount(self->pCount,eMode,fPreset, &lCts,pCon) != 1)
{
{
return 0;
}
/* print a message */
@@ -218,7 +238,7 @@
goto start;
}
/* no peak found or normal peak: continue at other side */
if( (i < 1) || (y[MAXPTS/2] > lMax/2) )
if( (i < 1) || (y[self->maxpts/2] > lMax/2) )
{
iSkip = 0;
}
@@ -227,7 +247,7 @@
/* next case: all of the peak in measured half:
find max value and skip the right half
*/
for(i = MAXPTS/2; i > 0; i--)
for(i = self->maxpts/2; i > 0; i--)
{
if(y[i] > lMax/2)
{
@@ -246,16 +266,16 @@
lMin = 100000;
lMax = -100000;
SCWrite(pCon,"Searching for high angle boundary..",eWarning);
for(i = MAXPTS/2; i < MAXPTS; i++)
for(i = self->maxpts/2; i < self->maxpts; i++)
{
/* drive motor */
fPos = fStart + (i - MAXPTS/2) * fStep;
fPos = fStart + (i - self->maxpts/2) * fStep;
fPos = in360(self,fPos);
if(maxDrive(pVar,pVarName,fPos,pCon) != 1)
{
return 0;
}
x[i] = pDriv->GetValue(pVar,pCon);
x[i] = readMPDrivable(pVar,pCon);
/* count */
if(maxCount(self->pCount,eMode,fPreset, &lCts,pCon) != 1)
{
@@ -292,18 +312,18 @@
iTop = i;
iTop++;
/* first case: peak is at high angle side */
if( (i > MAXPTS-2) && (lMax*0.5 > lMin) )
if( (i > self->maxpts-2) && (lMax*0.5 > lMin) )
{
goto start;
}
/* second case: no peak */
if( (iTop > MAXPTS-2) )
if( (iTop > self->maxpts-2) )
{
SCWrite(pCon,"ERROR: no peak found!",eError);
return 0;
}
/* third case: normal peak */
if(y[MAXPTS/2] >= 0.5*lMax)
if(y[self->maxpts/2] >= 0.5*lMax)
{
iTop--;
}
@@ -313,7 +333,7 @@
*/
else
{
for(i = MAXPTS/2; i < MAXPTS; i++)
for(i = self->maxpts/2; i < self->maxpts; i++)
{
if(y[i] > lMax/2)
{
@@ -325,7 +345,7 @@
}
} /* end of iSkip */
if( (iBot < 2) || (iTop > MAXPTS-2) || (lMax < lMin*2) )
if( (iBot < 2) || (iTop > self->maxpts-2) || (lMax < lMin*2) )
{
SCWrite(pCon,"ERROR: no peak found!",eError);
return 0;
@@ -420,11 +440,14 @@
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[])
{
@@ -434,7 +457,7 @@
double dVal;
float fStep, fPreset;
CounterMode eCount;
int iRet;
int iRet, iVal;
self = (pMax)pData;
assert(self);
@@ -446,9 +469,56 @@
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;