- Extended the Hdb adapter to support SICSdata - Made the simulated histogram memory driver work properly when data has been set. - Implemented the hfactory command - Removed hdbcommand which was never finsihed
962 lines
28 KiB
C
962 lines
28 KiB
C
/*---------------------------------------------------------------------------
|
|
|
|
M U L T I P L E M O T O R S
|
|
|
|
Code for maintaining a group of motors. This suuports
|
|
- aliases for motors
|
|
- named positions for motors
|
|
- a position history implemented as the special named position
|
|
back.
|
|
|
|
|
|
Mark Koennecke, December 1996
|
|
|
|
heavily reworked and simplified, Mark Koennecke, June 1997
|
|
|
|
added: defpos, recovernampos and made mumo save named positions
|
|
to the backup file. Mark Koennecke, March 2003
|
|
|
|
|
|
Copyright:
|
|
|
|
Labor fuer Neutronenstreuung
|
|
Paul Scherrer Institut
|
|
CH-5423 Villigen-PSI
|
|
|
|
|
|
The authors hereby grant permission to use, copy, modify, distribute,
|
|
and license this software and its documentation for any purpose, provided
|
|
that existing copyright notices are retained in all copies and that this
|
|
notice is included verbatim in any distributions. No written agreement,
|
|
license, or royalty fee is required for any of the authorized uses.
|
|
Modifications to this software may be copyrighted by their authors
|
|
and need not follow the licensing terms described here, provided that
|
|
the new terms are clearly indicated on the first page of each file where
|
|
they apply.
|
|
|
|
IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
|
|
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
|
ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
|
|
DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
|
|
POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
|
|
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE
|
|
IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
|
|
NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
|
|
MODIFICATIONS.
|
|
-----------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
#include <ctype.h>
|
|
#include <tcl.h>
|
|
#include "fortify.h"
|
|
#include "sics.h"
|
|
#include "conman.h"
|
|
#include "devexec.h"
|
|
#include "motor.h"
|
|
#include "obdes.h"
|
|
#include "splitter.h"
|
|
#include "nserver.h"
|
|
#include "stringdict.h"
|
|
#include "mumo.h"
|
|
#include "mumo.i"
|
|
/*-------------------------------------------------------------------------*/
|
|
static int SaveMumo(void *pData, char *name, FILE *fd)
|
|
{
|
|
pMulMot self = NULL;
|
|
char pCommand[512];
|
|
const char *pName = NULL;
|
|
|
|
self = (pMulMot)pData;
|
|
if(self == NULL)
|
|
{
|
|
return 0;
|
|
}
|
|
fprintf(fd,"#----- MultiMotor %s\n", name);
|
|
while((pName = StringDictGetNext(self->pNamPos,pCommand, 511)) != NULL)
|
|
{
|
|
if(strcmp(pName,"back") != 0)
|
|
{
|
|
fprintf(fd,"%s recovernampos %s %s\n",name,pName,pCommand);
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
pMulMot MakeMultiMotor(void)
|
|
{
|
|
pMulMot pNew = NULL;
|
|
|
|
pNew = (pMulMot)malloc(sizeof(MulMot));
|
|
if(!pNew)
|
|
{
|
|
return NULL;
|
|
}
|
|
memset(pNew,0,sizeof(MulMot));
|
|
|
|
/* the string Dictionaries */
|
|
pNew->pAlias = CreateStringDict();
|
|
pNew->pNamPos = CreateStringDict();
|
|
if( (!pNew->pAlias) || (!pNew->pNamPos))
|
|
{
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
|
|
pNew->pDes = CreateDescriptor("MulMot");
|
|
if(!pNew->pDes)
|
|
{
|
|
DeleteStringDict(pNew->pAlias);
|
|
DeleteStringDict(pNew->pNamPos);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->pDes->SaveStatus = SaveMumo;
|
|
|
|
/* the parameter array */
|
|
pNew->pParam = ObParCreate(1);
|
|
ObParInit(pNew->pParam,ACCESS,"accesscode",usUser,usMugger);
|
|
|
|
|
|
pNew->name = NULL;
|
|
return pNew;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
void KillMultiMotor(void *pData)
|
|
{
|
|
pMulMot self;
|
|
int i;
|
|
|
|
assert(pData);
|
|
|
|
self = (pMulMot)pData;
|
|
assert(strcmp(self->pDes->name,"MulMot") == 0);
|
|
|
|
/* remove string dictionaries */
|
|
if(self->pAlias)
|
|
{
|
|
DeleteStringDict(self->pAlias);
|
|
}
|
|
if(self->pNamPos)
|
|
{
|
|
DeleteStringDict(self->pNamPos);
|
|
}
|
|
|
|
/* free descriptor */
|
|
if(self->pDes)
|
|
{
|
|
DeleteDescriptor(self->pDes);
|
|
}
|
|
|
|
/* kill parameter array */
|
|
ObParDelete(self->pParam);
|
|
|
|
/* free name and myself */
|
|
if(self->name)
|
|
{
|
|
free(self->name);
|
|
}
|
|
free(self);
|
|
}
|
|
/*---------------------------------------------------------------------------
|
|
Multiple Motors have a funny and flexible syntax. Analysing the argc,
|
|
argv[] set will not do. Therefore this is done by some recursive descent
|
|
parsing.
|
|
*/
|
|
|
|
typedef struct __MYTOKEN {
|
|
char *pCommand;
|
|
char *pPtr;
|
|
int iCurrentToken;
|
|
char Token[80];
|
|
} sParser, *psParser;
|
|
/* define some TokenTypes */
|
|
#define END 0
|
|
#define INCREMENT 1
|
|
#define DECREMENT 2
|
|
#define NAMPOS 3
|
|
#define ALIAS 4
|
|
#define SYMBOL 5
|
|
#define NUMBER 6
|
|
#define EQUALITY 7
|
|
#define PLUS 8
|
|
#define MINUS 9
|
|
#define UNKNOWN 10
|
|
#define POSFIND 11
|
|
#define NAMALL 12
|
|
#define LIST 13
|
|
#define DEFPOS 14
|
|
#define RECOVERNAMPOS 15
|
|
#define GETPOS 16
|
|
/*-------------------------------------------------------------------------*/
|
|
static int GetNextToken(psParser self, pMulMot pDings)
|
|
{
|
|
char *pPtr;
|
|
int i;
|
|
|
|
pPtr = self->pPtr;
|
|
|
|
/* skip whitespace */
|
|
while( (*pPtr == ' ') || (*pPtr == '\t') )
|
|
{
|
|
pPtr++;
|
|
}
|
|
|
|
/* check for end */
|
|
if( (*pPtr == '\n') || (*pPtr == '\0') || (*pPtr == '\r') )
|
|
{
|
|
self->pPtr = pPtr;
|
|
self->Token[0] = *pPtr;
|
|
self->Token[1] = '\0';
|
|
self->iCurrentToken = END;
|
|
return END;
|
|
}
|
|
|
|
/* check equaL SIGN */
|
|
if(*pPtr == '=')
|
|
{
|
|
strcpy(self->Token,"=");
|
|
self->iCurrentToken = EQUALITY;
|
|
self->pPtr = pPtr +1;
|
|
return EQUALITY;
|
|
}
|
|
|
|
/* check + */
|
|
if(*pPtr == '+')
|
|
{
|
|
if(*(pPtr+1) == '+') /* increment */
|
|
{
|
|
strcpy(self->Token,"++");
|
|
self->iCurrentToken = INCREMENT;
|
|
self->pPtr = pPtr+2;
|
|
return INCREMENT;
|
|
}
|
|
else /* silly plus */
|
|
{
|
|
strcpy(self->Token,"+");
|
|
self->iCurrentToken = PLUS;
|
|
self->pPtr = pPtr + 1;
|
|
return PLUS;
|
|
}
|
|
}
|
|
|
|
/* check - */
|
|
if(*pPtr == '-')
|
|
{
|
|
if(*(pPtr+1) == '-') /* Decrement */
|
|
{
|
|
strcpy(self->Token,"--");
|
|
self->iCurrentToken = DECREMENT;
|
|
self->pPtr = pPtr+2;
|
|
return DECREMENT;
|
|
}
|
|
else /* silly minus */
|
|
{
|
|
strcpy(self->Token,"-");
|
|
self->iCurrentToken = MINUS;
|
|
self->pPtr = pPtr + 1;
|
|
return MINUS;
|
|
}
|
|
}
|
|
|
|
/* number */
|
|
if( (isdigit((int)*pPtr)) || (*pPtr == '.') )
|
|
{
|
|
i = 0;
|
|
while (isdigit((int)*pPtr) || (*pPtr == '.') )
|
|
{
|
|
self->Token[i] = *pPtr;
|
|
i++;
|
|
pPtr++;
|
|
}
|
|
self->Token[i] = '\0';
|
|
self->iCurrentToken = NUMBER;
|
|
self->pPtr = pPtr;
|
|
return NUMBER;
|
|
}
|
|
|
|
/* a Symbol ? */
|
|
if(isalnum((int)*pPtr))
|
|
{
|
|
i = 0;
|
|
while( (!isspace((int)*pPtr)) && (*pPtr != '=')
|
|
&& (*pPtr != '+') && (*pPtr != '-') &&(*pPtr != '\0')
|
|
&& (*pPtr != '\n') )
|
|
{
|
|
self->Token[i] = *pPtr;
|
|
i++;
|
|
pPtr++;
|
|
}
|
|
self->Token[i] = '\0';
|
|
self->pPtr = pPtr;
|
|
/* now it can be a named position, a motor alias or a keyword */
|
|
if(StringDictExists(pDings->pNamPos,self->Token))
|
|
{
|
|
self->iCurrentToken = NAMPOS;
|
|
return NAMPOS;
|
|
}
|
|
else if(StringDictExists(pDings->pAlias,self->Token))
|
|
{
|
|
self->iCurrentToken = ALIAS;
|
|
return ALIAS;
|
|
}
|
|
else if(strcmp(self->Token,"all") == 0)
|
|
{
|
|
self->iCurrentToken = NAMALL;
|
|
return NAMALL;
|
|
}
|
|
else if(strcmp(self->Token,"list") == 0)
|
|
{
|
|
self->iCurrentToken = LIST;
|
|
return LIST;
|
|
}
|
|
else if(strcmp(self->Token,"defpos") ==0)
|
|
{
|
|
self->iCurrentToken = DEFPOS;
|
|
return DEFPOS;
|
|
}
|
|
else if(strcmp(self->Token,"getpos") ==0)
|
|
{
|
|
self->iCurrentToken = GETPOS;
|
|
return GETPOS;
|
|
}
|
|
else if(strcmp(self->Token,"recovernampos") ==0)
|
|
{
|
|
self->iCurrentToken = RECOVERNAMPOS;
|
|
return RECOVERNAMPOS;
|
|
}
|
|
else
|
|
{
|
|
self->iCurrentToken = SYMBOL;
|
|
return SYMBOL;
|
|
}
|
|
}
|
|
return UNKNOWN;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void ListMulMot(char *name, SConnection *pCon, pMulMot self)
|
|
{
|
|
int i, iRet;
|
|
char pBueffel[512];
|
|
float fVal;
|
|
OutCode eOut = eValue;
|
|
const char *pNam = NULL;
|
|
char pMotName[132];
|
|
pMotor pMot = NULL;
|
|
|
|
sprintf(pBueffel,"Status listing for %s",name);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
|
|
/* scan through all aliases */
|
|
pNam = StringDictGetNext(self->pAlias,pMotName, 131);
|
|
while(pNam != NULL)
|
|
{
|
|
pMot = FindMotor(GetInterpreter(), pMotName);
|
|
assert(pMot); /* has been tested on definition */
|
|
iRet = MotorGetSoftPosition(pMot,pCon,&fVal);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pBueffel,"ERROR: Cannot read motor %s\n",
|
|
pMotName);
|
|
eOut = eError;
|
|
}
|
|
else
|
|
{
|
|
eOut = eValue;
|
|
sprintf(pBueffel,"%s.%s = %f\n",name,pNam, fVal);
|
|
}
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
pNam = StringDictGetNext(self->pAlias, pMotName, 131);
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int ParseAlias(psParser pParse, SConnection *pCon,
|
|
pMulMot self)
|
|
{
|
|
int i, iToken, iRet;
|
|
int iSign = 1;
|
|
int iInc = 0;
|
|
char pBueffel[132];
|
|
char pCommand[1064];
|
|
float fVal = 0, fIn = 0;
|
|
int iComplete;
|
|
pMotor pMot = NULL;
|
|
|
|
/* Okay first find the alias, the current token must still be
|
|
its name
|
|
*/
|
|
strcpy(pCommand,"run ");
|
|
iRet = StringDictGet(self->pAlias,pParse->Token,pBueffel,131);
|
|
/* that it is alias has been tested earlier */
|
|
assert(iRet == 1);
|
|
strcat(pCommand, pBueffel);
|
|
|
|
/* now find the value to handle */
|
|
iToken = GetNextToken(pParse,self);
|
|
iComplete = 0;
|
|
while(iToken != END)
|
|
{
|
|
switch(iToken)
|
|
{
|
|
case PLUS:
|
|
iSign = 1;
|
|
break;
|
|
case MINUS:
|
|
iSign = -1;
|
|
break;
|
|
case EQUALITY:
|
|
break;
|
|
case INCREMENT:
|
|
iInc = 1;
|
|
break;
|
|
case DECREMENT:
|
|
iSign = -1;
|
|
iInc = 1;
|
|
break;
|
|
case NUMBER:/* ey! Do some real work */
|
|
fIn = (float)atof(pParse->Token);
|
|
iComplete = 0;
|
|
if(iInc)
|
|
{
|
|
pMot = FindMotor(GetInterpreter(), pBueffel);
|
|
assert(pMot); /* existence shoul have been verified earlier */
|
|
MotorGetSoftPosition(pMot,pCon,&fVal);
|
|
fVal += iSign * fIn;
|
|
sprintf(pBueffel," %f ",fVal);
|
|
strcat(pCommand, pBueffel);
|
|
InterpExecute(GetInterpreter(),pCon,pCommand);
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
sprintf(pBueffel," %f ",iSign*fIn);
|
|
strcat(pCommand, pBueffel);
|
|
InterpExecute(GetInterpreter(),pCon,pCommand);
|
|
return 1;
|
|
}
|
|
return 1;
|
|
break;
|
|
default:
|
|
sprintf(pBueffel,"ERROR: Unexpected symbol %s",
|
|
pParse->Token);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
iToken = GetNextToken(pParse,self);
|
|
}
|
|
if(!iComplete)
|
|
{
|
|
SCWrite(pCon,"ERROR: Incomplete command",eError);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------
|
|
*/
|
|
static int MakeCurrentNamPos(char *name, SConnection *pCon,
|
|
pMulMot self)
|
|
{
|
|
const char *pAlias = NULL;
|
|
char pMotort[132];
|
|
char pCommand[1064];
|
|
pMotor pMot = NULL;
|
|
float fVal;
|
|
int iRet;
|
|
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
pCommand[0] = '\0';
|
|
pAlias = StringDictGetNext(self->pAlias, pMotort, 131);
|
|
while(pAlias != NULL)
|
|
{
|
|
strcat(pCommand, pMotort);
|
|
pMot = FindMotor(GetInterpreter(),pMotort);
|
|
assert(pMot); /* validity of alias has already been checked */
|
|
iRet = MotorGetSoftPosition(pMot,pCon,&fVal);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pCommand,"ERROR: failure to read motor %s, %s",
|
|
pMotort," named position not created");
|
|
SCWrite(pCon,pCommand,eError);
|
|
return 0;
|
|
}
|
|
sprintf(pMotort," %f ", fVal);
|
|
strcat(pCommand,pMotort);
|
|
pAlias = StringDictGetNext(self->pAlias, pMotort, 131);
|
|
}
|
|
if(StringDictExists(self->pNamPos,name))
|
|
{
|
|
StringDictUpdate(self->pNamPos,name,pCommand);
|
|
}
|
|
else
|
|
{
|
|
StringDictAddPair(self->pNamPos,name,pCommand);
|
|
}
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------
|
|
FindNamPos: checks if the current position is identical to a named
|
|
position. The comparison is done by getting the motors
|
|
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;
|
|
char pCurrCommand[1064], pTestCommand[1064];
|
|
const char *pAlias;
|
|
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);
|
|
pAlias = StringDictGetNext(self->pNamPos,pTestCommand,1063);
|
|
while(pAlias != NULL)
|
|
{
|
|
strcpy(pCurrCommand,pTestCommand);
|
|
pName = strtok(pCurrCommand," ");
|
|
iTest = 1;
|
|
while(pName != 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;
|
|
}
|
|
pAlias = StringDictGetNext(self->pNamPos,pTestCommand,1063);
|
|
}
|
|
DeleteStringDict(motCache);
|
|
/* not found */
|
|
return NULL;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int ParseDropPos(psParser pParse, SConnection *pCon,
|
|
pMulMot self)
|
|
{
|
|
int i, iToken, iRet;
|
|
char pBueffel[256];
|
|
float fVal = 0;
|
|
char pName[80];
|
|
char *pPtr = NULL;
|
|
|
|
iToken = GetNextToken(pParse,self);
|
|
while(iToken != END)
|
|
{
|
|
switch(iToken)
|
|
{
|
|
case EQUALITY:
|
|
break;
|
|
case NAMPOS:
|
|
/* we never want back to be killed ..... */
|
|
if(strcmp(pParse->Token,"back") == 0)
|
|
{
|
|
SCWrite(pCon,
|
|
"ERROR: You are NOT allowed to deleted system position BACK",
|
|
eError);
|
|
return 0;
|
|
}
|
|
/* find NamPos and kill it */
|
|
iRet = StringDictDelete(self->pNamPos,pParse->Token);
|
|
return 1;
|
|
break;
|
|
case NAMALL:
|
|
/* kill all named positions except back */
|
|
iRet = StringDictGet(self->pNamPos,"back",pBueffel,256);
|
|
DeleteStringDict(self->pNamPos);
|
|
self->pNamPos = CreateStringDict();
|
|
if(!self->pNamPos)
|
|
{
|
|
SCWrite(pCon,
|
|
"ERROR: severe! out of memory in mumo drop",
|
|
eError);
|
|
return 0;
|
|
}
|
|
if(iRet)
|
|
{
|
|
StringDictAddPair(self->pNamPos,"back",pBueffel);
|
|
}
|
|
return 1;
|
|
break;
|
|
default:
|
|
sprintf(pBueffel,"ERROR: Unexpected symbol %s",
|
|
pParse->Token);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int CheckPermission(SConnection *pCon, pMulMot self)
|
|
{
|
|
int iAccess;
|
|
char pBueffel[132];
|
|
|
|
iAccess = (int)ObVal(self->pParam,ACCESS);
|
|
if(SCMatchRights(pCon, iAccess))
|
|
{
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
sprintf(pBueffel,"ERROR: NO permission to drive %s",
|
|
self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
/* ------------------------------------------------------------------------*/
|
|
static int ParseNamPos(psParser pParse, SConnection *pCon,
|
|
SicsInterp *pSics, pMulMot self)
|
|
{
|
|
int iToken, iRet;
|
|
char pBueffel[256];
|
|
char pCommand[1064];
|
|
char *pPtr;
|
|
|
|
/* is the user allowed to do ANYTHING ? */
|
|
iRet = CheckPermission(pCon,self);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
strcpy(pCommand,"run ");
|
|
pPtr = pCommand + strlen("run ");
|
|
iRet = StringDictGet(self->pNamPos,pParse->Token,pPtr, 1050);
|
|
if(iRet)
|
|
{
|
|
InterpExecute(GetInterpreter(),pCon,pCommand);
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
sprintf(pCommand,"ERROR: named position %s NOT found",pParse->Token);
|
|
SCWrite(pCon,pCommand,eError);
|
|
return 0;
|
|
}
|
|
return 0; /* not reached */
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int ParseDefPos(SicsInterp *pSics,psParser pPP,
|
|
pMulMot self, SConnection *pCon)
|
|
{
|
|
pMotor pMot = NULL;
|
|
char pError[132], motorName[80], command[1024], namPos[80];
|
|
int iToken, status;
|
|
float fVal;
|
|
|
|
|
|
iToken = GetNextToken(pPP,self);
|
|
if(iToken != SYMBOL) /* we want a name here */
|
|
{
|
|
sprintf(pError,"ERROR: Invalid Token %s in ParsePos",pPP->Token);
|
|
SCWrite(pCon,pError,eError);
|
|
return 0;
|
|
}
|
|
|
|
strncpy(namPos,pPP->Token,79);
|
|
iToken = GetNextToken(pPP,self);
|
|
command[0] = '\0';
|
|
while(iToken != END)
|
|
{
|
|
if(iToken != ALIAS)
|
|
{
|
|
sprintf(command,"ERROR: expected motor alias, got: %s", pPP->Token);
|
|
SCWrite(pCon,command,eError);
|
|
return 0;
|
|
}
|
|
StringDictGet(self->pAlias,pPP->Token,motorName,79);
|
|
strcat(command, motorName);
|
|
strcat(command," ");
|
|
iToken = GetNextToken(pPP,self);
|
|
if(iToken != NUMBER)
|
|
{
|
|
sprintf(command,"ERROR: expected number, got: %s", pPP->Token);
|
|
SCWrite(pCon,command,eError);
|
|
return 0;
|
|
}
|
|
strcat(command,pPP->Token);
|
|
strcat(command," ");
|
|
iToken = GetNextToken(pPP,self);
|
|
}
|
|
|
|
StringDictAddPair(self->pNamPos,namPos,command);
|
|
return 1;
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
static void RecoverNamPos(pMulMot self, int argc, char *argv[])
|
|
{
|
|
char pCommand[512];
|
|
|
|
Arg2Text(argc-1,&argv[1],pCommand, 511);
|
|
if(StringDictExists(self->pNamPos,argv[0]))
|
|
{
|
|
StringDictUpdate(self->pNamPos,argv[0],pCommand);
|
|
}
|
|
else
|
|
{
|
|
StringDictAddPair(self->pNamPos,argv[0],pCommand);
|
|
}
|
|
}
|
|
/*---------------------------------------------------------------------------
|
|
MultiWrapper is the user interface to a multi motor unit. It supports the
|
|
following syntax, where DingsBums is the name of the unit:
|
|
|
|
DingsBums - lists position of all registered motors.
|
|
DingsBums NamPos - drives of to named position
|
|
DingsBums [alias = num] - drives alias to num. num supports
|
|
decrement, increment and simple values.
|
|
DingsBums pos name - makes the current position a named
|
|
position with name name.
|
|
DingsBums getpos - gets the current named position
|
|
DingsBums drop name - deletes the current named position
|
|
name.
|
|
DingsBums drop all - drops all named positions (except back)
|
|
DingsBums list - lists all named positions.
|
|
DingsBums recovernampos nam bla.... - internal command to recover
|
|
saved named positions
|
|
|
|
|
|
*/
|
|
|
|
|
|
int MultiWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
char pBueffel[512];
|
|
char pError[132];
|
|
sParser MyParser;
|
|
int iToken;
|
|
int iRet;
|
|
pMulMot self;
|
|
int iAlias = 0;
|
|
const char *pPtr;
|
|
Tcl_DString tString;
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
assert(pData);
|
|
|
|
self = (pMulMot)pData;
|
|
assert(strcmp(self->pDes->name,"MulMot")== 0);
|
|
|
|
/* setup */
|
|
argtolower(argc,argv);
|
|
iRet = Arg2Text(argc,argv,pBueffel,511);
|
|
if(!iRet)
|
|
{
|
|
SCWrite(pCon,"Command line to long",eError);
|
|
return 0;
|
|
}
|
|
MyParser.pPtr = &pBueffel[0];
|
|
MyParser.pCommand = pBueffel;
|
|
|
|
/* ignore first Token: that is my name ! */
|
|
iToken = GetNextToken(&MyParser,self);
|
|
|
|
/* now loop through my Tokens */
|
|
iToken = GetNextToken(&MyParser,self);
|
|
if(iToken == END)
|
|
{
|
|
ListMulMot(argv[0],pCon,self);
|
|
return 1;
|
|
}
|
|
while( iToken != END)
|
|
{
|
|
switch(iToken)
|
|
{
|
|
case END:
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
case NAMPOS:
|
|
/* do a named position, current token is it */
|
|
if(strcmp(MyParser.Token,"back")!= 0)
|
|
{
|
|
MakeCurrentNamPos("back",pCon,self);
|
|
}
|
|
iRet = ParseNamPos(&MyParser,pCon,pSics,self);
|
|
if(iRet)
|
|
{
|
|
SCSendOK(pCon);
|
|
SCparChange(pCon);
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
break;
|
|
case ALIAS:
|
|
iRet = CheckPermission(pCon, self);
|
|
if(iAlias == 0)
|
|
{
|
|
MakeCurrentNamPos("back",pCon,self);
|
|
iAlias = 1;
|
|
}
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
iRet = ParseAlias(&MyParser,pCon,self);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
break;
|
|
case GETPOS:
|
|
pPtr = FindNamPos(self,pCon);
|
|
if(pPtr != NULL)
|
|
{
|
|
sprintf(pBueffel,"%s.nampos = %s",argv[0], pPtr);
|
|
}
|
|
else
|
|
{
|
|
sprintf(pBueffel,"%s.nampos = undefined",argv[0]);
|
|
}
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
return 1;
|
|
case DEFPOS:
|
|
SCparChange(pCon);
|
|
return ParseDefPos(pSics,&MyParser,self,pCon);
|
|
break;
|
|
case LIST:
|
|
pPtr = NULL;
|
|
Tcl_DStringInit(&tString);
|
|
sprintf(pBueffel,"%s list of known named positions \n",
|
|
argv[0]);
|
|
Tcl_DStringAppend(&tString,pBueffel,strlen(pBueffel));
|
|
StringDictKillScan(self->pNamPos);
|
|
pPtr = StringDictGetNext(self->pNamPos,pError,131);
|
|
while(pPtr != NULL)
|
|
{
|
|
Tcl_DStringAppend(&tString,(char *)pPtr,-1);
|
|
Tcl_DStringAppend (&tString," \n",-1);
|
|
pPtr = StringDictGetNext(self->pNamPos,
|
|
pError,131);
|
|
}
|
|
SCWrite(pCon,Tcl_DStringValue(&tString),eValue);
|
|
Tcl_DStringFree(&tString);
|
|
return 1;
|
|
break;
|
|
case SYMBOL:
|
|
if(strcmp(MyParser.Token,"pos") == 0)
|
|
{
|
|
iRet = CheckPermission(pCon,self);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
iToken = GetNextToken(&MyParser, self);
|
|
if((iToken == SYMBOL) || (iToken == NAMPOS))
|
|
{
|
|
MakeCurrentNamPos(MyParser.Token,pCon,self);
|
|
SCparChange(pCon);
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
sprintf(pError,"ERROR: %s no valid named position name",
|
|
MyParser.Token);
|
|
SCWrite(pCon,pError,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
if(strcmp(MyParser.Token,"drop") == 0)
|
|
{
|
|
iRet = CheckPermission(pCon,self);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
SCparChange(pCon);
|
|
return ParseDropPos(&MyParser, pCon, self);
|
|
}
|
|
}
|
|
if(strcmp(MyParser.Token,"find") == 0)
|
|
{
|
|
pPtr = NULL;
|
|
pPtr = FindNamPos(self,pCon);
|
|
if(pPtr)
|
|
{
|
|
sprintf(pBueffel,"%s.namedposition = %s",argv[0],pPtr);
|
|
}
|
|
else
|
|
{
|
|
sprintf(pBueffel,"%s.namedposition = UNKNOWN",argv[0]);
|
|
}
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
return 1;
|
|
}
|
|
sprintf(pError,"ERROR: Unknown Token %s",MyParser.Token);
|
|
SCWrite(pCon,pError,eError);
|
|
return 0;
|
|
case RECOVERNAMPOS:
|
|
/*
|
|
This is not meant to be user command but a facility to read
|
|
back data from sattus file. This is why the error checking
|
|
is not happening
|
|
*/
|
|
RecoverNamPos(self,argc-2,&argv[2]);
|
|
return 1;
|
|
default:
|
|
SCWrite(pCon,"ERROR: Parse Error",eError);
|
|
return 0;
|
|
}
|
|
iToken = GetNextToken(&MyParser,self);
|
|
}
|
|
return 1;
|
|
}
|
|
|