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

- Committing first working version of new AMOR settings module

Browse Source
This commit is contained in:
koennecke
2005-10-05 07:20:17 +00:00
parent 8f6f6f9f80
commit ca69fe5101
28 changed files with 3607 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
mumo.c
View File

@ -505,7 +505,7 @@ static int SaveMumo(void *pData, char *name, FILE *fd)
current value and compare it with the stored ones for
all currently stored named positions.
*/
-------------------------------------------------------------------------*/
const char *FindNamPos(pMulMot self, SConnection *pCon)
{
int iRet, iTest;
@ -514,6 +514,33 @@ static int SaveMumo(void *pData, char *name, FILE *fd)
char *pName, *pVal;
float f1, f2;
pMotor pMot = NULL;
pStringDict motCache = NULL;
/*
* create cache of motor positions
*/
motCache = CreateStringDict();
if(motCache == NULL){
SCWrite(pCon,"ERROR: out of memory in FindNamPos",eError);
return NULL;
}
StringDictKillScan(self->pAlias);
pAlias = StringDictGetNext(self->pAlias,pCurrCommand,1023);
while(pAlias != NULL){
pMot = FindMotor(pServ->pSics,pCurrCommand);
if(pMot != NULL){
iRet = MotorGetSoftPosition(pMot,pCon,&f1);
if(!iRet){
sprintf(pTestCommand,
"ERROR: failed to get motor position for %s", pName);
SCWrite(pCon,pTestCommand,eError);
return NULL;
}
snprintf(pTestCommand,1023,"%f",f1);
StringDictAddPair(motCache,pCurrCommand,pTestCommand);
}
pAlias = StringDictGetNext(self->pAlias,pCurrCommand,1023);
}
/* scan named position list */
StringDictKillScan(self->pNamPos);
@ -524,43 +551,27 @@ static int SaveMumo(void *pData, char *name, FILE *fd)
pName = strtok(pCurrCommand," ");
iTest = 1;
while(pName != NULL)
{
pVal = strtok(NULL," ");
pMot = FindMotor(pServ->pSics,pName);
if(pMot)
{
iRet = MotorGetSoftPosition(pMot,pCon,&f1);
if(!iRet)
{
sprintf(pTestCommand,
"ERROR: failed to get motor position for %s", pName);
SCWrite(pCon,pTestCommand,eError);
return NULL;
}
sscanf(pVal,"%f",&f2);
f1 = f1 - f2;
if(f1 < 0.)
f1 = - f1;
if(f1 > 0.03)
{
iTest--;
break;
}
}
else
{
SCWrite(pCon,"ERROR: internal error in FindNamPos, no motor",
eError);
return NULL;
}
pName = strtok(NULL," ");
{
pVal = strtok(NULL," ");
StringDictGetAsNumber(motCache,pName,&f1);
sscanf(pVal,"%f",&f2);
f1 = f1 - f2;
if(f1 < 0.)
f1 = - f1;
if(f1 > 0.03)
{
iTest--;
break;
}
pName = strtok(NULL," ");
}
if(iTest == 1 && (strcmp(pAlias,"back") != 0) ) {
DeleteStringDict(motCache);
return pAlias;
}
if(iTest == 1 && (strcmp(pAlias,"back") != 0) )
return pAlias;
pAlias = StringDictGetNext(self->pNamPos,pTestCommand,1063);
}
DeleteStringDict(motCache);
/* not found */
return NULL;
}