- Adapted indenation to new agreed upon system
- Fixed bad status in poldi zug driver
This commit is contained in:
koennecke
738
swmotor.c
738
swmotor.c
@ -23,555 +23,495 @@
|
||||
#include "swmotor.h"
|
||||
#include "swmotor.i"
|
||||
|
||||
typedef struct __MoDriv {
|
||||
/* general motor driver interface
|
||||
fields. REQUIRED!
|
||||
*/
|
||||
float fUpper; /* upper limit */
|
||||
float fLower; /* lower limit */
|
||||
char *name;
|
||||
int (*GetPosition)(void *self,float *fPos);
|
||||
int (*RunTo)(void *self, float fNewVal);
|
||||
int (*GetStatus)(void *self);
|
||||
void (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
|
||||
int (*TryAndFixIt)(void *self,int iError, float fNew);
|
||||
int (*Halt)(void *self);
|
||||
int (*GetDriverPar)(void *self, char *name,
|
||||
float *value);
|
||||
int (*SetDriverPar)(void *self,SConnection *pCon,
|
||||
char *name, float newValue);
|
||||
void (*ListDriverPar)(void *self, char *motorName,
|
||||
SConnection *pCon);
|
||||
void (*KillPrivate)(void *self);
|
||||
|
||||
|
||||
/* EL-734 specific fields */
|
||||
int iPort;
|
||||
char *hostname;
|
||||
int iChannel;
|
||||
int iMotor;
|
||||
void *EL734struct;
|
||||
int iMSR;
|
||||
} EL734Driv;
|
||||
typedef struct __MoDriv {
|
||||
/* general motor driver interface
|
||||
fields. REQUIRED!
|
||||
*/
|
||||
float fUpper; /* upper limit */
|
||||
float fLower; /* lower limit */
|
||||
char *name;
|
||||
int (*GetPosition) (void *self, float *fPos);
|
||||
int (*RunTo) (void *self, float fNewVal);
|
||||
int (*GetStatus) (void *self);
|
||||
void (*GetError) (void *self, int *iCode, char *buffer, int iBufLen);
|
||||
int (*TryAndFixIt) (void *self, int iError, float fNew);
|
||||
int (*Halt) (void *self);
|
||||
int (*GetDriverPar) (void *self, char *name, float *value);
|
||||
int (*SetDriverPar) (void *self, SConnection * pCon,
|
||||
char *name, float newValue);
|
||||
void (*ListDriverPar) (void *self, char *motorName, SConnection * pCon);
|
||||
void (*KillPrivate) (void *self);
|
||||
|
||||
|
||||
/* EL-734 specific fields */
|
||||
int iPort;
|
||||
char *hostname;
|
||||
int iChannel;
|
||||
int iMotor;
|
||||
void *EL734struct;
|
||||
int iMSR;
|
||||
} EL734Driv;
|
||||
/*========================================================================
|
||||
We start of by implementing the interface functions for the various
|
||||
interfaces this module has to implement.
|
||||
==========================================================================*/
|
||||
static int SWHalt(void *pData)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
pSWmot self = (pSWmot) pData;
|
||||
assert(self);
|
||||
return self->pMaster->pDrivInt->Halt(self->pMaster);
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int SWCheckLimits(void *pData, float fVal, char *error, int iErrLen)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
pSWmot self = (pSWmot) pData;
|
||||
assert(self);
|
||||
return self->pMaster->pDrivInt->CheckLimits(self->pMaster,
|
||||
fVal,error,iErrLen);
|
||||
fVal, error, iErrLen);
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int SWCheckStatus(void *pData, SConnection *pCon)
|
||||
static int SWCheckStatus(void *pData, SConnection * pCon)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
pSWmot self = (pSWmot) pData;
|
||||
assert(self);
|
||||
return self->pMaster->pDrivInt->CheckStatus(self->pMaster,
|
||||
pCon);
|
||||
return self->pMaster->pDrivInt->CheckStatus(self->pMaster, pCon);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int SWSaveStatus(void *pData, char *name, FILE *fd)
|
||||
static int SWSaveStatus(void *pData, char *name, FILE * fd)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
pSWmot self = (pSWmot) pData;
|
||||
assert(self);
|
||||
|
||||
fprintf(fd,"%s savedValue = %f\n", name,
|
||||
self->positions[self->myNumber]);
|
||||
fprintf(fd, "%s savedValue = %f\n", name,
|
||||
self->positions[self->myNumber]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static void *SWGetInterface(void *pData, int ID)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
pSWmot self = (pSWmot) pData;
|
||||
assert(self);
|
||||
|
||||
if(ID == DRIVEID)
|
||||
{
|
||||
if (ID == DRIVEID) {
|
||||
return self->pDriv;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------*/
|
||||
static float SWGetValue(void *pData, SConnection *pCon)
|
||||
static float SWGetValue(void *pData, SConnection * pCon)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
pSWmot self = (pSWmot) pData;
|
||||
float fVal;
|
||||
assert(self);
|
||||
|
||||
/*
|
||||
we are not selected: return stored data:
|
||||
*/
|
||||
if(self->myNumber != self->selectedMotor[0])
|
||||
{
|
||||
SCWrite(pCon,"WARNING: motor not activem returning stored value",
|
||||
we are not selected: return stored data:
|
||||
*/
|
||||
if (self->myNumber != self->selectedMotor[0]) {
|
||||
SCWrite(pCon, "WARNING: motor not activem returning stored value",
|
||||
eWarning);
|
||||
return self->positions[self->myNumber];
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
fVal = self->pMaster->pDrivInt->GetValue(self->pMaster, pCon);
|
||||
self->positions[self->myNumber] = fVal;
|
||||
return fVal;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static long SWSetValue(void *pData, SConnection *pCon, float fVal)
|
||||
static long SWSetValue(void *pData, SConnection * pCon, float fVal)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
pSWmot self = (pSWmot) pData;
|
||||
char pCommand[256], pError[132];
|
||||
int status;
|
||||
EL734Driv *pElli;
|
||||
EL734Driv *pElli;
|
||||
Tcl_Interp *pTcl;
|
||||
|
||||
assert(self);
|
||||
self->errCode = 1;
|
||||
|
||||
/*
|
||||
first case: we are already selected
|
||||
*/
|
||||
if(self->myNumber == self->selectedMotor[0])
|
||||
{
|
||||
return self->pMaster->pDrivInt->SetValue(self->pMaster,pCon,fVal);
|
||||
}
|
||||
else
|
||||
{
|
||||
first case: we are already selected
|
||||
*/
|
||||
if (self->myNumber == self->selectedMotor[0]) {
|
||||
return self->pMaster->pDrivInt->SetValue(self->pMaster, pCon, fVal);
|
||||
} else {
|
||||
/*
|
||||
second case: switch to the requested motor, do a reference run
|
||||
and then execute Set command
|
||||
*/
|
||||
sprintf(pCommand,"Switching to motor number %d", self->myNumber);
|
||||
SCWrite(pCon,pCommand,eWarning);
|
||||
sprintf(pCommand,"%s %d",self->switchFunc, self->myNumber);
|
||||
pTcl = (Tcl_Interp *)pServ->pSics->pTcl;
|
||||
status = Tcl_Eval(pTcl,pCommand);
|
||||
strncpy(pError,pTcl->result, 131);
|
||||
if(status != TCL_OK || strstr(pError,"OK") == NULL)
|
||||
{
|
||||
sprintf(pCommand,"ERROR: %s while switching motor", pError);
|
||||
SCWrite(pCon,pCommand, eError);
|
||||
second case: switch to the requested motor, do a reference run
|
||||
and then execute Set command
|
||||
*/
|
||||
sprintf(pCommand, "Switching to motor number %d", self->myNumber);
|
||||
SCWrite(pCon, pCommand, eWarning);
|
||||
sprintf(pCommand, "%s %d", self->switchFunc, self->myNumber);
|
||||
pTcl = (Tcl_Interp *) pServ->pSics->pTcl;
|
||||
status = Tcl_Eval(pTcl, pCommand);
|
||||
strncpy(pError, pTcl->result, 131);
|
||||
if (status != TCL_OK || strstr(pError, "OK") == NULL) {
|
||||
sprintf(pCommand, "ERROR: %s while switching motor", pError);
|
||||
SCWrite(pCon, pCommand, eError);
|
||||
self->errCode = -1001;
|
||||
return HWFault;
|
||||
}
|
||||
self->selectedMotor[0] = self->myNumber;
|
||||
|
||||
/*
|
||||
switch done! Start a reference run
|
||||
*/
|
||||
switch done! Start a reference run
|
||||
*/
|
||||
SicsWait(10);
|
||||
SCWrite(pCon,"Standby, starting reference run... ", eWarning);
|
||||
pElli = (EL734Driv *)self->pMaster->pDriver;
|
||||
sprintf(pCommand,"R %d\r",pElli->iMotor);
|
||||
status = EL734_SendCmnd(&(pElli->EL734struct),
|
||||
pCommand,
|
||||
pError, 131);
|
||||
if(status != 1)
|
||||
{
|
||||
sprintf(pCommand,"ERROR: %s while trying to start reference run",
|
||||
SCWrite(pCon, "Standby, starting reference run... ", eWarning);
|
||||
pElli = (EL734Driv *) self->pMaster->pDriver;
|
||||
sprintf(pCommand, "R %d\r", pElli->iMotor);
|
||||
status = EL734_SendCmnd(&(pElli->EL734struct), pCommand, pError, 131);
|
||||
if (status != 1) {
|
||||
sprintf(pCommand, "ERROR: %s while trying to start reference run",
|
||||
pError);
|
||||
SCWrite(pCon,pCommand,eError);
|
||||
SCWrite(pCon, pCommand, eError);
|
||||
self->errCode = -1002;
|
||||
return HWFault;
|
||||
}
|
||||
}
|
||||
/*
|
||||
now loop forever until reference run is done. This is either when the
|
||||
motors stops being busy or when the user interrupts.
|
||||
*/
|
||||
sprintf(pCommand,"SS %d\r",pElli->iMotor);
|
||||
for( ; ;)
|
||||
{
|
||||
status = EL734_SendCmnd(&(pElli->EL734struct),
|
||||
pCommand,
|
||||
pError, 131);
|
||||
if(status != 1)
|
||||
{
|
||||
sprintf(pCommand,"ERROR: %s during reference run",
|
||||
pError);
|
||||
SCWrite(pCon,pCommand,eError);
|
||||
self->errCode = -1003;
|
||||
return HWFault;
|
||||
}
|
||||
if(strstr(pError,"?BSY") == NULL)
|
||||
break;
|
||||
if(SCGetInterrupt(pCon) != eContinue)
|
||||
{
|
||||
self->errCode = -1004;
|
||||
SCWrite(pCon,"ERROR: user interrupted reference run",eError);
|
||||
return HWFault;
|
||||
}
|
||||
SicsWait(2);
|
||||
now loop forever until reference run is done. This is either when the
|
||||
motors stops being busy or when the user interrupts.
|
||||
*/
|
||||
sprintf(pCommand, "SS %d\r", pElli->iMotor);
|
||||
for (;;) {
|
||||
status = EL734_SendCmnd(&(pElli->EL734struct),
|
||||
pCommand, pError, 131);
|
||||
if (status != 1) {
|
||||
sprintf(pCommand, "ERROR: %s during reference run", pError);
|
||||
SCWrite(pCon, pCommand, eError);
|
||||
self->errCode = -1003;
|
||||
return HWFault;
|
||||
}
|
||||
if (strstr(pError, "?BSY") == NULL)
|
||||
break;
|
||||
if (SCGetInterrupt(pCon) != eContinue) {
|
||||
self->errCode = -1004;
|
||||
SCWrite(pCon, "ERROR: user interrupted reference run", eError);
|
||||
return HWFault;
|
||||
}
|
||||
SicsWait(2);
|
||||
}
|
||||
|
||||
/*
|
||||
now this is finished. We can really start driving the motor
|
||||
*/
|
||||
SCWrite(pCon,"Reference run completed, starting to drive motor..",
|
||||
now this is finished. We can really start driving the motor
|
||||
*/
|
||||
SCWrite(pCon, "Reference run completed, starting to drive motor..",
|
||||
eWarning);
|
||||
return self->pMaster->pDrivInt->SetValue(self->pMaster,pCon,fVal);
|
||||
return self->pMaster->pDrivInt->SetValue(self->pMaster, pCon, fVal);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------*/
|
||||
static void KillSWFull(void *pData)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
if(self == NULL)
|
||||
pSWmot self = (pSWmot) pData;
|
||||
if (self == NULL)
|
||||
return;
|
||||
|
||||
if(self->pDriv)
|
||||
|
||||
if (self->pDriv)
|
||||
free(self->pDriv);
|
||||
if(self->pDes)
|
||||
if (self->pDes)
|
||||
DeleteDescriptor(self->pDes);
|
||||
if(self->selectedMotor)
|
||||
if (self->selectedMotor)
|
||||
free(self->selectedMotor);
|
||||
if(self->switchFunc)
|
||||
if (self->switchFunc)
|
||||
free(self->switchFunc);
|
||||
free(self);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------*/
|
||||
static void KillSWHalf(void *pData)
|
||||
{
|
||||
pSWmot self = (pSWmot)pData;
|
||||
if(self)
|
||||
pSWmot self = (pSWmot) pData;
|
||||
if (self)
|
||||
free(self);
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------
|
||||
Alright, now the interpreter functions follow
|
||||
Usage:
|
||||
MakeSWMotor master switchFunc slave1 slave2 slave3
|
||||
-----------------------------------------------------------------------*/
|
||||
int MakeSWMotor(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
int MakeSWMotor(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pSWmot sw1, sw2, sw3;
|
||||
char pBueffel[256];
|
||||
int i, status;
|
||||
|
||||
/*
|
||||
check that we have enough arguments
|
||||
*/
|
||||
if(argc < 6)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: insufficient number of arguments to MakeSWMotor",
|
||||
check that we have enough arguments
|
||||
*/
|
||||
if (argc < 6) {
|
||||
SCWrite(pCon, "ERROR: insufficient number of arguments to MakeSWMotor",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
allocate a new data structure
|
||||
*/
|
||||
sw1 = (pSWmot)malloc(sizeof(SWmot));
|
||||
if(sw1 == NULL)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
|
||||
allocate a new data structure
|
||||
*/
|
||||
sw1 = (pSWmot) malloc(sizeof(SWmot));
|
||||
if (sw1 == NULL) {
|
||||
SCWrite(pCon, "ERROR: out of memory in MakeSWMotor", eError);
|
||||
return 0;
|
||||
}
|
||||
memset(sw1,0,sizeof(SWmot));
|
||||
|
||||
memset(sw1, 0, sizeof(SWmot));
|
||||
|
||||
/*
|
||||
fill it up with stuff
|
||||
*/
|
||||
fill it up with stuff
|
||||
*/
|
||||
sw1->pDes = CreateDescriptor("Sparbroetchen");
|
||||
sw1->pDriv = CreateDrivableInterface();
|
||||
sw1->selectedMotor = (int *)malloc(sizeof(int));
|
||||
if(!sw1->pDes || ! sw1->pDriv || !sw1->selectedMotor)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
|
||||
sw1->selectedMotor = (int *) malloc(sizeof(int));
|
||||
if (!sw1->pDes || !sw1->pDriv || !sw1->selectedMotor) {
|
||||
SCWrite(pCon, "ERROR: out of memory in MakeSWMotor", eError);
|
||||
return 0;
|
||||
}
|
||||
sw1->selectedMotor[0] = -1;
|
||||
sw1->pMaster = FindMotor(pSics,argv[1]);
|
||||
if(!sw1->pMaster)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: cannot find master motor %s", argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
sw1->pMaster = FindMotor(pSics, argv[1]);
|
||||
if (!sw1->pMaster) {
|
||||
sprintf(pBueffel, "ERROR: cannot find master motor %s", argv[1]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
sw1->switchFunc = strdup(argv[2]);
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
strcpy(sw1->slaves[i],argv[3+i]);
|
||||
}
|
||||
for (i = 0; i < 3; i++) {
|
||||
strcpy(sw1->slaves[i], argv[3 + i]);
|
||||
}
|
||||
|
||||
/*
|
||||
initialize function pointers
|
||||
*/
|
||||
initialize function pointers
|
||||
*/
|
||||
sw1->pDes->SaveStatus = SWSaveStatus;
|
||||
sw1->pDes->GetInterface = SWGetInterface;
|
||||
sw1->pDriv->GetValue = SWGetValue;
|
||||
sw1->pDriv->SetValue = SWSetValue;
|
||||
sw1->pDriv->Halt = SWHalt;
|
||||
sw1->pDriv->Halt = SWHalt;
|
||||
sw1->pDriv->CheckStatus = SWCheckStatus;
|
||||
sw1->pDriv->CheckLimits = SWCheckLimits;
|
||||
|
||||
/*
|
||||
create clones of the new data structure ofr the other slaves
|
||||
*/
|
||||
sw2 = (pSWmot)malloc(sizeof(SWmot));
|
||||
sw3 = (pSWmot)malloc(sizeof(SWmot));
|
||||
if(!sw2 || !sw2)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
|
||||
create clones of the new data structure ofr the other slaves
|
||||
*/
|
||||
sw2 = (pSWmot) malloc(sizeof(SWmot));
|
||||
sw3 = (pSWmot) malloc(sizeof(SWmot));
|
||||
if (!sw2 || !sw2) {
|
||||
SCWrite(pCon, "ERROR: out of memory in MakeSWMotor", eError);
|
||||
return 0;
|
||||
}
|
||||
memcpy(sw2,sw1,sizeof(SWmot));
|
||||
memcpy(sw2, sw1, sizeof(SWmot));
|
||||
sw2->myNumber = 1;
|
||||
memcpy(sw3,sw1,sizeof(SWmot));
|
||||
memcpy(sw3, sw1, sizeof(SWmot));
|
||||
sw3->myNumber = 2;
|
||||
|
||||
/*
|
||||
install commands
|
||||
*/
|
||||
install commands
|
||||
*/
|
||||
status = AddCommand(pSics, argv[3], SWMotorAction, KillSWFull, sw1);
|
||||
if(!status)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: command %s already exists!",argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
if (!status) {
|
||||
sprintf(pBueffel, "ERROR: command %s already exists!", argv[3]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
status = AddCommand(pSics, argv[4], SWMotorAction, KillSWHalf, sw2);
|
||||
if(!status)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: command %s already exists!",argv[4]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
if (!status) {
|
||||
sprintf(pBueffel, "ERROR: command %s already exists!", argv[4]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
status = AddCommand(pSics, argv[5], SWMotorAction, KillSWHalf, sw3);
|
||||
if(!status)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: command %s already exists!",argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
if (!status) {
|
||||
sprintf(pBueffel, "ERROR: command %s already exists!", argv[3]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------
|
||||
some prototypes from functions implemented in motor.c
|
||||
--------------------------------------------------------------------*/
|
||||
void MotorListLL(pMotor self, SConnection *pCon);
|
||||
void MotorListLL(pMotor self, SConnection * pCon);
|
||||
void MotorReset(pMotor pM);
|
||||
/*---------------------------------------------------------------------*/
|
||||
int SWMotorAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
int SWMotorAction(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
char pBueffel[512];
|
||||
TokenList *pList = NULL;
|
||||
TokenList *pCurrent;
|
||||
TokenList *pName;
|
||||
int iRet;
|
||||
pSWmot self = NULL;
|
||||
float fValue;
|
||||
long lID;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
assert(pData);
|
||||
|
||||
self = (pSWmot)pData;
|
||||
|
||||
/* create Tokenlist */
|
||||
argtolower(argc,argv);
|
||||
pList = SplitArguments(argc,argv);
|
||||
if(!pList)
|
||||
{
|
||||
SCWrite(pCon,"Error parsing argument list in SWMotorAction",eError);
|
||||
char pBueffel[512];
|
||||
TokenList *pList = NULL;
|
||||
TokenList *pCurrent;
|
||||
TokenList *pName;
|
||||
int iRet;
|
||||
pSWmot self = NULL;
|
||||
float fValue;
|
||||
long lID;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
assert(pData);
|
||||
|
||||
self = (pSWmot) pData;
|
||||
|
||||
/* create Tokenlist */
|
||||
argtolower(argc, argv);
|
||||
pList = SplitArguments(argc, argv);
|
||||
if (!pList) {
|
||||
SCWrite(pCon, "Error parsing argument list in SWMotorAction", eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* first argument can be one of list, reset or parameter name */
|
||||
pCurrent = pList->pNext;
|
||||
if (!pCurrent) { /* no argument, print value */
|
||||
fValue = self->pDriv->GetValue(self, pCon);
|
||||
if (fValue < -990.) {
|
||||
sprintf(pBueffel, "Error obtaining position for %s", argv[0]);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* first argument can be one of list, reset or parameter name */
|
||||
pCurrent = pList->pNext;
|
||||
if(!pCurrent) /* no argument, print value */
|
||||
{
|
||||
fValue = self->pDriv->GetValue(self,pCon);
|
||||
if(fValue < -990.)
|
||||
{
|
||||
sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
sprintf(pBueffel,"%s = %f",argv[0],fValue);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* check for list */
|
||||
if(strcmp(pCurrent->text,"list") == 0)
|
||||
{
|
||||
MotorListLL(self->pMaster,pCon);
|
||||
sprintf(pBueffel, "%s = %f", argv[0], fValue);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* check for list */
|
||||
if (strcmp(pCurrent->text, "list") == 0) {
|
||||
MotorListLL(self->pMaster, pCon);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
} /* check for reset */
|
||||
else if (strcmp(pCurrent->text, "reset") == 0) {
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
sprintf(pBueffel, "Insufficient privilege to reset %s", argv[0]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
MotorReset(self->pMaster);
|
||||
DeleteTokenList(pList);
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
} else if (strcmp(pCurrent->text, "savedValue") == 0) {
|
||||
pCurrent = pCurrent->pNext;
|
||||
if (!pCurrent) {
|
||||
/*
|
||||
print Value
|
||||
*/
|
||||
sprintf(pBueffel, "%s.savedValue = %f", argv[0],
|
||||
self->positions[self->myNumber]);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
} else {
|
||||
/*
|
||||
set saveValue, must be a numeric parameter
|
||||
*/
|
||||
if (pCurrent->Type == eInt) {
|
||||
fValue = (float) pCurrent->iVal;
|
||||
} else if (pCurrent->Type == eFloat) {
|
||||
fValue = pCurrent->fVal;
|
||||
} else {
|
||||
sprintf(pBueffel, "Wrong argument type for %s %s ",
|
||||
argv[0], "saveValue");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
self->positions[self->myNumber] = fValue;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
} else if (strcmp(pCurrent->text, "selected") == 0) {
|
||||
pCurrent = pCurrent->pNext;
|
||||
if (!pCurrent) {
|
||||
/*
|
||||
print Value
|
||||
*/
|
||||
sprintf(pBueffel, "%s.selected = %d", argv[0],
|
||||
self->selectedMotor[0]);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
} else {
|
||||
/*
|
||||
set selected, must be a numeric parameter
|
||||
*/
|
||||
if (pCurrent->Type == eInt) {
|
||||
fValue = (float) pCurrent->iVal;
|
||||
} else if (pCurrent->Type == eFloat) {
|
||||
fValue = pCurrent->fVal;
|
||||
} else {
|
||||
sprintf(pBueffel, "Wrong argument type for %s %s ",
|
||||
argv[0], "selected");
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
self->selectedMotor[0] = (int) fValue;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
} else { /* one of the parameter commands or error left now */
|
||||
|
||||
pName = pCurrent;
|
||||
pCurrent = pCurrent->pNext;
|
||||
if (!pCurrent) { /* no third par: print val */
|
||||
/* deal with position first */
|
||||
if (strcmp(pName->text, "position") == 0) {
|
||||
fValue = self->pDriv->GetValue(self, pCon);
|
||||
if (fValue < 999.) {
|
||||
sprintf(pBueffel, "Error obtaining position for %s", argv[0]);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
sprintf(pBueffel, "%s.SoftPosition = %f", argv[0], fValue);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
} /* check for reset */
|
||||
else if(strcmp(pCurrent->text,"reset") == 0)
|
||||
{
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
{
|
||||
sprintf(pBueffel,"Insufficient privilege to reset %s",
|
||||
argv[0]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
MotorReset(self->pMaster);
|
||||
DeleteTokenList(pList);
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
else if(strcmp(pCurrent->text,"savedValue") == 0)
|
||||
{
|
||||
pCurrent = pCurrent->pNext;
|
||||
if(!pCurrent)
|
||||
{
|
||||
/*
|
||||
print Value
|
||||
*/
|
||||
sprintf(pBueffel,"%s.savedValue = %f", argv[0],
|
||||
self->positions[self->myNumber]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
/*
|
||||
set saveValue, must be a numeric parameter
|
||||
*/
|
||||
if(pCurrent->Type == eInt)
|
||||
{
|
||||
fValue = (float)pCurrent->iVal;
|
||||
}
|
||||
else if(pCurrent->Type == eFloat)
|
||||
{
|
||||
fValue = pCurrent->fVal;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"Wrong argument type for %s %s ",
|
||||
argv[0],"saveValue");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
self->positions[self->myNumber] = fValue;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else if(strcmp(pCurrent->text,"selected") == 0)
|
||||
{
|
||||
pCurrent = pCurrent->pNext;
|
||||
if(!pCurrent)
|
||||
{
|
||||
/*
|
||||
print Value
|
||||
*/
|
||||
sprintf(pBueffel,"%s.selected = %d", argv[0],
|
||||
self->selectedMotor[0]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
/*
|
||||
set selected, must be a numeric parameter
|
||||
*/
|
||||
if(pCurrent->Type == eInt)
|
||||
{
|
||||
fValue = (float)pCurrent->iVal;
|
||||
}
|
||||
else if(pCurrent->Type == eFloat)
|
||||
{
|
||||
fValue = pCurrent->fVal;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"Wrong argument type for %s %s ",
|
||||
argv[0],"selected");
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
self->selectedMotor[0] = (int)fValue;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else /* one of the parameter commands or error left now */
|
||||
{
|
||||
pName = pCurrent;
|
||||
pCurrent = pCurrent->pNext;
|
||||
if(!pCurrent) /* no third par: print val */
|
||||
{
|
||||
/* deal with position first */
|
||||
if(strcmp(pName->text,"position") == 0)
|
||||
{
|
||||
fValue = self->pDriv->GetValue(self,pCon);
|
||||
if(fValue < 999.)
|
||||
{
|
||||
sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
sprintf(pBueffel,"%s.SoftPosition = %f",argv[0],fValue);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
iRet = MotorGetPar(self->pMaster,pName->text,&fValue);
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"Parameter %s not found ",pName->text);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel, " %s.%s = %f",argv[0],pName->text,fValue);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else /* set */
|
||||
{ /* set command */
|
||||
/* parameter must be numerical value */
|
||||
if(pCurrent->Type == eInt)
|
||||
{
|
||||
fValue = (float)pCurrent->iVal;
|
||||
}
|
||||
else if(pCurrent->Type == eFloat)
|
||||
{
|
||||
fValue = pCurrent->fVal;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"Wrong argument type for %s %s set",
|
||||
argv[0],pName->text);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
iRet = MotorSetPar(self->pMaster,pCon,pName->text,fValue);
|
||||
DeleteTokenList(pList);
|
||||
if(iRet)
|
||||
SCSendOK(pCon);
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
iRet = MotorGetPar(self->pMaster, pName->text, &fValue);
|
||||
if (!iRet) {
|
||||
sprintf(pBueffel, "Parameter %s not found ", pName->text);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
} else {
|
||||
sprintf(pBueffel, " %s.%s = %f", argv[0], pName->text, fValue);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
DeleteTokenList(pList);
|
||||
return 1;
|
||||
}
|
||||
} else { /* set */
|
||||
/* set command */
|
||||
/* parameter must be numerical value */
|
||||
if (pCurrent->Type == eInt) {
|
||||
fValue = (float) pCurrent->iVal;
|
||||
} else if (pCurrent->Type == eFloat) {
|
||||
fValue = pCurrent->fVal;
|
||||
} else {
|
||||
sprintf(pBueffel, "Wrong argument type for %s %s set",
|
||||
argv[0], pName->text);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
DeleteTokenList(pList);
|
||||
return 0;
|
||||
}
|
||||
iRet = MotorSetPar(self->pMaster, pCon, pName->text, fValue);
|
||||
DeleteTokenList(pList);
|
||||
if (iRet)
|
||||
SCSendOK(pCon);
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user