860 lines
25 KiB
C
860 lines
25 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;
|
|
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "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) {
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: Cannot read motor %s\n", pMotName);
|
|
eOut = eError;
|
|
} else {
|
|
eOut = eValue;
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "%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);
|
|
strlcat(pCommand, pBueffel,1024);
|
|
|
|
/* 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;
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, " %f ", fVal);
|
|
strlcat(pCommand, pBueffel,1024);
|
|
InterpExecute(GetInterpreter(), pCon, pCommand);
|
|
return 1;
|
|
} else {
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, " %f ", iSign * fIn);
|
|
strlcat(pCommand, pBueffel,1024);
|
|
InterpExecute(GetInterpreter(), pCon, pCommand);
|
|
return 1;
|
|
}
|
|
return 1;
|
|
break;
|
|
default:
|
|
snprintf(pBueffel,sizeof(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) {
|
|
strlcat(pCommand, pMotort,1024);
|
|
pMot = FindMotor(GetInterpreter(), pMotort);
|
|
assert(pMot); /* validity of alias has already been checked */
|
|
iRet = MotorGetSoftPosition(pMot, pCon, &fVal);
|
|
if (!iRet) {
|
|
snprintf(pCommand,1024, "ERROR: failure to read motor %s, %s",
|
|
pMotort, " named position not created");
|
|
SCWrite(pCon, pCommand, eError);
|
|
return 0;
|
|
}
|
|
snprintf(pMotort,sizeof(pMotort)-1, " %f ", fVal);
|
|
strlcat(pCommand, pMotort,1024);
|
|
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 *pVal, *pName;
|
|
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) {
|
|
snprintf(pTestCommand,sizeof(pTestCommand)-1,
|
|
"ERROR: failed to get motor position for %s", pMot->name);
|
|
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:
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "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 {
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "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 {
|
|
snprintf(pCommand,sizeof(pCommand)-1, "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 */
|
|
snprintf(pError,sizeof(pError)-1, "ERROR: Invalid Token %s in ParsePos", pPP->Token);
|
|
SCWrite(pCon, pError, eError);
|
|
return 0;
|
|
}
|
|
|
|
strlcpy(namPos, pPP->Token, 79);
|
|
iToken = GetNextToken(pPP, self);
|
|
command[0] = '\0';
|
|
while (iToken != END) {
|
|
if (iToken != ALIAS) {
|
|
snprintf(command,sizeof(command)-1, "ERROR: expected motor alias, got: %s", pPP->Token);
|
|
SCWrite(pCon, command, eError);
|
|
return 0;
|
|
}
|
|
StringDictGet(self->pAlias, pPP->Token, motorName, 79);
|
|
strlcat(command, motorName,1024);
|
|
strlcat(command, " ",1024);
|
|
iToken = GetNextToken(pPP, self);
|
|
if (iToken != NUMBER) {
|
|
snprintf(command,sizeof(command)-1, "ERROR: expected number, got: %s", pPP->Token);
|
|
SCWrite(pCon, command, eError);
|
|
return 0;
|
|
}
|
|
strlcat(command, pPP->Token,1024);
|
|
strlcat(command, " ",1024);
|
|
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 defpos name [alias value..] - makes a named
|
|
position with name name. The par is a list of alias value
|
|
pairs with the appropriate positions for 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) {
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.nampos = %s", argv[0], pPtr);
|
|
} else {
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "%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);
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "%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 {
|
|
snprintf(pError,sizeof(pError)-1, "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) {
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.namedposition = %s", argv[0], pPtr);
|
|
} else {
|
|
snprintf(pBueffel,sizeof(pBueffel)-1, "%s.namedposition = UNKNOWN", argv[0]);
|
|
}
|
|
SCWrite(pCon, pBueffel, eValue);
|
|
return 1;
|
|
}
|
|
snprintf(pError,sizeof(pError)-1, "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 status 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;
|
|
}
|