f3853c20f0
- Fixed synchronisation issues - Fixed hsitogram memory writing from nxscript - Started module for writing SICS interfaces in Tcl - Fixed a bug in scan, which allowed to corrupt files - Fixed memory problems in napi5
1325 lines
38 KiB
C
1325 lines
38 KiB
C
/*------------------------------------------------------------------------
|
|
M O T O R S
|
|
|
|
This file implements the SICS motor handling. This is the logical
|
|
level, the nitty gritty hardware interface is in the driver.
|
|
|
|
|
|
|
|
Mark Koennecke, November 1996
|
|
revised: Mark Koennecke, June 1997
|
|
callback added: Mark Koennecke, August 1997
|
|
endscript facility added: Mark Koennecke, August 2002
|
|
Modified to support driver parameters, Mark Koennecke, January 2003
|
|
|
|
TODO: currently motor drivers have to be installed in MakeMotor
|
|
and remembered in KillMotor. Sort this some day!
|
|
|
|
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.
|
|
|
|
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 <math.h>
|
|
#include <tcl.h>
|
|
#include "fortify.h"
|
|
#include "sics.h"
|
|
#include "devexec.h"
|
|
#include "motor.h"
|
|
#include "splitter.h"
|
|
#include "status.h"
|
|
#include "servlog.h"
|
|
#include "ecbdriv.h"
|
|
|
|
/*-------------------------------------------------------------------------
|
|
some lokal defines
|
|
*/
|
|
#define ZEROINACTIVE 0
|
|
#define INTCONT 0.
|
|
#define INTSCAN 1.
|
|
#define INTBATCH 2.
|
|
#define INTHALT 3.
|
|
|
|
#define HLOW 0
|
|
#define HUPP 1
|
|
#define SLOW 2
|
|
#define SUPP 3
|
|
#define SZERO 4
|
|
#define FIX 5
|
|
#define INT 6
|
|
#define PREC 7
|
|
#define USRIGHTS 8
|
|
#define SPEED 9
|
|
#define SIGN 10
|
|
#define ECOUNT 11
|
|
/*------------------------------------------------------------------------
|
|
a tiny structure used in CallBack work
|
|
*/
|
|
typedef struct {
|
|
float fVal;
|
|
char *pName;
|
|
} MotCallback;
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
static void *MotorGetInterface(void *pData, int iID)
|
|
{
|
|
pMotor self = NULL;
|
|
|
|
self = (pMotor)pData;
|
|
assert(self);
|
|
if(iID == DRIVEID)
|
|
{
|
|
return self->pDrivInt;
|
|
}
|
|
else if(iID == CALLBACKINTERFACE)
|
|
{
|
|
return self->pCall;
|
|
}
|
|
return NULL;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int MotorHalt(void *sulf)
|
|
{
|
|
pMotor self;
|
|
|
|
assert(sulf);
|
|
self = (pMotor)sulf;
|
|
|
|
/* reduce the error count by 1. This is because the driver is
|
|
expected to return an error when the motor had been stopped.
|
|
However, a stop is usually a consequence of a user intervention
|
|
or program logic. This prevents to a false motor alarm when the
|
|
motor was repeatedly stopped for other reasons.
|
|
*/
|
|
self->pDrivInt->iErrorCount--;
|
|
if(self->pDrivInt->iErrorCount < 0)
|
|
self->pDrivInt->iErrorCount = 0;
|
|
|
|
return self->pDriver->Halt((void *)self->pDriver);
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
static int MotorLimits(void *sulf, float fVal, char *error, int iErrLen)
|
|
{
|
|
float fHard;
|
|
pMotor self;
|
|
|
|
assert(sulf);
|
|
|
|
self = (pMotor)sulf;
|
|
|
|
return MotorCheckBoundary(self,fVal,&fHard,error,iErrLen);
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
static float MotorGetValue(void *pData, SConnection *pCon)
|
|
{
|
|
int iRet;
|
|
float fVal = 0.;
|
|
|
|
assert(pData);
|
|
iRet = MotorGetHardPosition((pMotor)pData,pCon,&fVal);
|
|
if(iRet != OKOK)
|
|
{
|
|
fVal = -9999999.99;
|
|
}
|
|
return fVal;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int MotorSaveStatus(void *pData, char *name, FILE *fd)
|
|
{
|
|
pMotor self = NULL;
|
|
char pBueffel[512];
|
|
|
|
assert(pData);
|
|
assert(fd);
|
|
|
|
self= (pMotor)pData;
|
|
fprintf(fd,"# Motor %s\n",name);
|
|
sprintf(pBueffel,"%s sign %f\n",name,ObVal(self->ParArray,SIGN));
|
|
fputs(pBueffel,fd);
|
|
sprintf(pBueffel,"%s SoftZero %f\n",name,ObVal(self->ParArray,SZERO));
|
|
fputs(pBueffel,fd);
|
|
sprintf(pBueffel,"%s SoftLowerLim %f\n",name,ObVal(self->ParArray,SLOW));
|
|
fputs(pBueffel,fd);
|
|
sprintf(pBueffel,"%s SoftUpperLim %f\n",name,ObVal(self->ParArray,SUPP));
|
|
fputs(pBueffel,fd);
|
|
sprintf(pBueffel,"%s Fixed %f\n",name,ObVal(self->ParArray,FIX));
|
|
fputs(pBueffel,fd);
|
|
sprintf(pBueffel,"%s InterruptMode %f\n",name,ObVal(self->ParArray,INT));
|
|
fputs(pBueffel,fd);
|
|
sprintf(pBueffel,"%s precision %f\n",name,ObVal(self->ParArray,PREC));
|
|
fputs(pBueffel,fd);
|
|
sprintf(pBueffel,"%s AccessCode %f\n",name,ObVal(self->ParArray,USRIGHTS));
|
|
fputs(pBueffel,fd);
|
|
return 1;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static float absf(float f)
|
|
{
|
|
if(f < 0.)
|
|
{
|
|
return -f;
|
|
}
|
|
else
|
|
{
|
|
return f;
|
|
}
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void MotorInterrupt(SConnection *pCon, int iVal)
|
|
{
|
|
if(SCGetInterrupt(pCon) < iVal)
|
|
{
|
|
SCSetInterrupt(pCon,iVal);
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------
|
|
Refactor
|
|
--------------------------------------------------------------------------*/
|
|
static int MotorStatus(void *sulf, SConnection *pCon)
|
|
{
|
|
float fHard;
|
|
pMotor self;
|
|
int iRet,i, iCode;
|
|
static int iRetry = 0;
|
|
char pError[132];
|
|
char pBueffel[256];
|
|
static int iPosFault = 0;
|
|
MotCallback sCall;
|
|
|
|
|
|
assert(sulf);
|
|
|
|
self = (pMotor)sulf;
|
|
|
|
iRet = self->pDriver->GetStatus(self->pDriver);
|
|
fHard = MotorGetValue((void *)self,pCon);
|
|
if(fHard < -9999990)
|
|
{
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
|
|
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
|
|
if(iRet == MOTFAIL)
|
|
{
|
|
sprintf(pBueffel,"WARNING: %s on %s",pError,self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
|
|
iRetry = 0;
|
|
return HWFault;
|
|
}
|
|
else if(iRet == MOTREDO)
|
|
{
|
|
iRetry++;
|
|
if(iRetry >= 3)
|
|
{
|
|
iRetry = 0;
|
|
return HWFault;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
iRetry = 0;
|
|
return HWBusy;
|
|
}
|
|
}
|
|
|
|
/* invoke callback */
|
|
sCall.fVal = fHard;
|
|
sCall.pName = self->name;
|
|
InvokeCallBack(self->pCall, MOTDRIVE, &sCall);
|
|
|
|
/* motor thinks he is done */
|
|
if( (iRet == OKOK) || (iRet == HWIdle))
|
|
{
|
|
MotorGetSoftPosition(self,pCon,&sCall.fVal);
|
|
InvokeCallBack(self->pCall, MOTEND, &sCall);
|
|
self->fPosition = fHard;
|
|
if(absf(fHard - self->fTarget) > ObVal(self->ParArray,PREC))
|
|
{
|
|
sprintf(pBueffel,"WARNING: %s off position by %f",
|
|
self->name, absf(fHard - self->fTarget));
|
|
SCWrite(pCon,pBueffel, eWarning);
|
|
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
|
|
iRetry = 0;
|
|
return HWPosFault;
|
|
}
|
|
iRetry = 0;
|
|
return HWIdle;
|
|
}
|
|
/* motor suggests a fault */
|
|
else if(iRet == HWFault)
|
|
{
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
|
|
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
|
|
if(iRet == MOTFAIL)
|
|
{
|
|
sprintf(pBueffel,"ERROR: %s on %s",pError,self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
|
|
iRetry = 0;
|
|
return HWFault;
|
|
}
|
|
else if(iRet == MOTREDO)
|
|
{
|
|
self->pDriver->RunTo(self->pDriver,self->fTarget);
|
|
iRetry++;
|
|
if(iRetry >= 3)
|
|
{
|
|
iRetry = 0;
|
|
return HWFault;
|
|
}
|
|
return HWBusy;
|
|
}
|
|
else
|
|
{
|
|
iRetry = 0;
|
|
return HWBusy;
|
|
}
|
|
}
|
|
/* a positioning fault */
|
|
else if(iRet == HWPosFault)
|
|
{
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
|
|
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
|
|
if(iRet == MOTFAIL)
|
|
{
|
|
sprintf(pBueffel,"ERROR: %s on %s",pError,self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
|
|
iRetry = 0;
|
|
return HWFault;
|
|
}
|
|
else if(iRet == MOTREDO)
|
|
{
|
|
iPosFault++;
|
|
if(iPosFault == 2)
|
|
{
|
|
iPosFault = 0;
|
|
iRetry = 0;
|
|
return HWPosFault;
|
|
}
|
|
self->pDriver->RunTo(self->pDriver,self->fTarget);
|
|
return HWBusy;
|
|
}
|
|
else
|
|
{
|
|
return HWBusy;
|
|
}
|
|
}
|
|
else if(iRet == HWWarn)
|
|
{
|
|
self->pDriver->GetError(self->pDriver,&iCode,pError,131);
|
|
sprintf(pBueffel,"WARNING: %s on %s",pError,self->name);
|
|
SCWrite(pCon,pBueffel,eStatus);
|
|
return HWIdle;
|
|
}
|
|
iRetry = 0;
|
|
return iRet;
|
|
}
|
|
/*---------------------------------------------------------------------------*/ pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv)
|
|
{
|
|
pMotor pM = NULL;
|
|
|
|
assert(drivername);
|
|
assert(pDriv);
|
|
assert(name);
|
|
|
|
/* get memory */
|
|
pM = (pMotor)malloc(sizeof(Motor));
|
|
if(!pM)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
|
|
/* create and initialize parameters */
|
|
pM->ParArray = ObParCreate(12);
|
|
if(!pM->ParArray)
|
|
{
|
|
free(pM);
|
|
return NULL;
|
|
}
|
|
ObParInit(pM->ParArray,HLOW,"hardlowerlim",pDriv->fLower,usInternal);
|
|
ObParInit(pM->ParArray,HUPP,"hardupperlim",pDriv->fUpper,usInternal);
|
|
ObParInit(pM->ParArray,SLOW,"softlowerlim",pDriv->fLower,usUser);
|
|
ObParInit(pM->ParArray,SUPP,"softupperlim",pDriv->fUpper,usUser);
|
|
ObParInit(pM->ParArray,SZERO,"softzero",ZEROINACTIVE,usUser);
|
|
ObParInit(pM->ParArray,FIX,"fixed",-1,usUser);
|
|
ObParInit(pM->ParArray,INT,"interruptmode",INTCONT,usMugger);
|
|
ObParInit(pM->ParArray,PREC,"precision",0.01,usMugger);
|
|
ObParInit(pM->ParArray,USRIGHTS,"accesscode",(float)usUser,usMugger);
|
|
ObParInit(pM->ParArray,SPEED,"speed",0.02,usInternal);
|
|
ObParInit(pM->ParArray,SIGN,"sign",1.0,usMugger);
|
|
ObParInit(pM->ParArray,ECOUNT,"failafter",3.0,usMugger);
|
|
pDriv->GetPosition(pDriv,&(pM->fPosition));
|
|
pM->fTarget = pM->fPosition;
|
|
pM->endScriptID = 0;
|
|
|
|
/* copy arguments */
|
|
pM->pDriver = pDriv;
|
|
pM->drivername = strdup(drivername);
|
|
pM->name = strdup(name);
|
|
|
|
|
|
/* initialise object descriptor */
|
|
pM->pDescriptor = CreateDescriptor("Motor");
|
|
if(!pM->pDescriptor)
|
|
{
|
|
ObParDelete(pM->ParArray);
|
|
free(pM);
|
|
return NULL;
|
|
}
|
|
pM->pDescriptor->GetInterface = MotorGetInterface;
|
|
pM->pDescriptor->SaveStatus = MotorSaveStatus;
|
|
|
|
/* initialise Drivable interface */
|
|
pM->pDrivInt = CreateDrivableInterface();
|
|
if(!pM->pDrivInt)
|
|
{
|
|
DeleteDescriptor(pM->pDescriptor);
|
|
ObParDelete(pM->ParArray);
|
|
free(pM);
|
|
return NULL;
|
|
}
|
|
pM->pDrivInt->SetValue = MotorRun;
|
|
pM->pDrivInt->CheckLimits = MotorLimits;
|
|
pM->pDrivInt->CheckStatus = MotorStatus;
|
|
pM->pDrivInt->GetValue = MotorGetValue;
|
|
pM->pDrivInt->Halt = MotorHalt;
|
|
|
|
/* initialise callback interface */
|
|
pM->pCall = CreateCallBackInterface();
|
|
if(!pM->pCall)
|
|
{
|
|
MotorKill(pM);
|
|
return NULL;
|
|
}
|
|
|
|
/* done */
|
|
return pM;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
|
|
extern void KillPiPiezo(void *pData);
|
|
|
|
void MotorKill(void *self)
|
|
{
|
|
pMotor pM;
|
|
assert(self);
|
|
|
|
pM = (pMotor)self;
|
|
|
|
if(pM->name)
|
|
free(pM->name);
|
|
|
|
if(pM->pDrivInt)
|
|
{
|
|
free(pM->pDrivInt);
|
|
}
|
|
|
|
if(pM->pCall)
|
|
{
|
|
DeleteCallBackInterface(pM->pCall);
|
|
}
|
|
|
|
/* kill driver */
|
|
if(pM->drivername)
|
|
{ /* edit here to include more drivers */
|
|
if(strcmp(pM->drivername,"EL734") == 0)
|
|
{
|
|
KillEL734((void *)pM->pDriver);
|
|
}
|
|
else if(strcmp(pM->drivername,"EL734DC") == 0)
|
|
{
|
|
KillEL734((void *)pM->pDriver);
|
|
}
|
|
else if(strcmp(pM->drivername,"SIM") == 0)
|
|
{
|
|
KillSIM((void *)pM->pDriver);
|
|
}
|
|
else if(strcmp(pM->drivername,"PIPIEZO") == 0)
|
|
{
|
|
KillPiPiezo((void *)pM->pDriver);
|
|
}
|
|
else if(strcmp(pM->drivername,"ECB") == 0)
|
|
{
|
|
KillECBMotor( (void *)pM->pDriver);
|
|
}
|
|
free(pM->drivername);
|
|
}
|
|
|
|
/* get rid of parameter space */
|
|
if(pM->ParArray)
|
|
{
|
|
ObParDelete(pM->ParArray);
|
|
}
|
|
|
|
/* kill Descriptor */
|
|
DeleteDescriptor(pM->pDescriptor);
|
|
|
|
free(pM);
|
|
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
int MotorGetPar(pMotor self, char *name, float *fVal)
|
|
{
|
|
ObPar *pPar = NULL;
|
|
assert(self);
|
|
|
|
pPar = ObParFind(self->ParArray,name);
|
|
if(pPar)
|
|
{
|
|
*fVal = pPar->fVal;
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
/* can still be position */
|
|
if(strcmp(name,"position") == 0)
|
|
{
|
|
*fVal = self->fPosition;
|
|
return 1;
|
|
}
|
|
else if(strcmp(name,"target") == 0)
|
|
{
|
|
*fVal = self->fTarget;
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
/*
|
|
check for a driver parameter
|
|
*/
|
|
if(self->pDriver->GetDriverPar != NULL)
|
|
{
|
|
return self->pDriver->GetDriverPar(self->pDriver,name,fVal);
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
int MotorSetPar(pMotor self, SConnection *pCon, char *name, float fVal)
|
|
{
|
|
ObPar *pPar = NULL;
|
|
char pBueffel[512];
|
|
int iRet;
|
|
float fLimit, fOld, fChange;
|
|
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
/*
|
|
try set driver parameters
|
|
*/
|
|
if(self->pDriver->SetDriverPar != NULL)
|
|
{
|
|
iRet = self->pDriver->SetDriverPar(self->pDriver,pCon,name,fVal);
|
|
if(iRet == 1)
|
|
{
|
|
SCparChange(pCon);
|
|
return iRet;
|
|
}
|
|
}
|
|
|
|
|
|
if(strcmp(name,"softzero") == 0)
|
|
{
|
|
/* set it first, this also tests the necessary privileges */
|
|
fOld = ObVal(self->ParArray,SZERO);
|
|
iRet = ObParSet(self->ParArray,self->name,name,fVal,pCon);
|
|
if(!iRet)
|
|
{
|
|
return iRet;
|
|
}
|
|
/* shift the limits by the difference between old and new */
|
|
fChange = fVal - fOld;
|
|
/* upper limit */
|
|
fLimit = ObVal(self->ParArray,SUPP);
|
|
fLimit -= fChange;
|
|
ObParSet(self->ParArray,self->name,"softupperlim",fLimit,pCon);
|
|
/* lower limit */
|
|
fLimit = ObVal(self->ParArray,SLOW);
|
|
fLimit -= fChange;
|
|
ObParSet(self->ParArray,self->name,"softlowerlim",fLimit,pCon);
|
|
SCparChange(pCon);
|
|
|
|
return 1;
|
|
}
|
|
|
|
iRet = ObParSet(self->ParArray,self->name,name,fVal,pCon);
|
|
if(strcmp(name,"sign") == 0)
|
|
{
|
|
if((absf(fVal)-1.0) > 0.01)
|
|
{
|
|
SCWrite(pCon,"ERROR: Invalid Sign Value",eError);
|
|
return 0;
|
|
}
|
|
if(fVal < 0.0)
|
|
{
|
|
ObParInit(self->ParArray,SLOW,"softlowerlim",
|
|
self->pDriver->fUpper*fVal,usUser);
|
|
ObParInit(self->ParArray,SUPP,"softupperlim",
|
|
self->pDriver->fLower*fVal,usUser);
|
|
ObParInit(self->ParArray,SZERO,"softzero",ZEROINACTIVE,usUser);
|
|
}
|
|
}
|
|
SCparChange(pCon);
|
|
|
|
return iRet;
|
|
}
|
|
/*---------------------------------------------------------------------------
|
|
MotorCheckBoundary checks for violation of boundary conditions and
|
|
transforms from SoftCoordinates to hard coordinates.
|
|
*/
|
|
|
|
int MotorCheckBoundary(pMotor self, float fVal, float *fNew,
|
|
char *pError, int iErrLen)
|
|
{
|
|
float fHard;
|
|
float fZero;
|
|
char pBueffel[512];
|
|
|
|
assert(self);
|
|
|
|
/* check for fixed */
|
|
if(ObVal(self->ParArray,FIX) >= 0)
|
|
{
|
|
sprintf(pBueffel,"Motor %s is Fixed",self->name);
|
|
strncpy(pError,pBueffel,iErrLen);
|
|
return 0; /* is this an error? */
|
|
}
|
|
|
|
/* check against software boundaries */
|
|
if(fVal > ObVal(self->ParArray,SUPP))
|
|
{
|
|
sprintf(pBueffel,"%f violates upper software limit %f on %s",
|
|
fVal, ObVal(self->ParArray,SUPP),self->name);
|
|
strncpy(pError,pBueffel,iErrLen);
|
|
return 0;
|
|
}
|
|
if(fVal < ObVal(self->ParArray,SLOW))
|
|
{
|
|
sprintf(pBueffel,"%f violates lower software limit %f on %s",
|
|
fVal,ObVal(self->ParArray,SLOW),self->name );
|
|
strncpy(pError,pBueffel,iErrLen);
|
|
return 0;
|
|
}
|
|
|
|
/* correct for zero point */
|
|
fZero = ObVal(self->ParArray,SZERO);
|
|
fZero = -fZero;
|
|
fHard = fVal - fZero;
|
|
|
|
/* apply sign */
|
|
fHard = fHard*ObVal(self->ParArray,SIGN);
|
|
|
|
/* check for hardware limits */
|
|
if(fHard > ObVal(self->ParArray,HUPP))
|
|
{
|
|
sprintf(pBueffel,"%f violates upper hardware limit %f on %s",
|
|
fVal,ObVal(self->ParArray,HUPP),self->name);
|
|
strncpy(pError,pBueffel,iErrLen);
|
|
return 0;
|
|
}
|
|
if(fHard < ObVal(self->ParArray,HLOW))
|
|
{
|
|
sprintf(pBueffel,"%f violates lower hardware limit %f on %s",
|
|
fVal,ObVal(self->ParArray,HLOW),self->name);
|
|
strncpy(pError,pBueffel,iErrLen);
|
|
return 0;
|
|
}
|
|
|
|
*fNew = fHard;
|
|
return 1;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
long MotorRun(void *sulf, SConnection *pCon, float fNew)
|
|
{
|
|
float fHard;
|
|
int i, iRet, iCode;
|
|
char pBueffel[512];
|
|
char pError[132];
|
|
pMotor self;
|
|
long lTime;
|
|
float fDelta;
|
|
|
|
self = (pMotor)(sulf);
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
/* check if I'am allowed to move this motor */
|
|
if(!SCMatchRights(pCon,(int)ObVal(self->ParArray,USRIGHTS)))
|
|
{
|
|
sprintf(pBueffel,"ERROR: You are not authorised to move motor %s",
|
|
self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
SCSetInterrupt(pCon,eAbortBatch);
|
|
return 0;
|
|
}
|
|
|
|
/* check boundaries first */
|
|
iRet = MotorCheckBoundary(self,fNew,&fHard,pBueffel,511);
|
|
if(!iRet)
|
|
{
|
|
SCWrite(pCon,pBueffel,eStatus);
|
|
SCSetInterrupt(pCon,eAbortOperation);
|
|
return 0;
|
|
}
|
|
|
|
/* check our error count and interrupt if to much */
|
|
iCode = (int)ObVal(self->ParArray,ECOUNT);
|
|
if(self->pDrivInt->iErrorCount > iCode)
|
|
{
|
|
/* big alarm */
|
|
ServerWriteGlobal("ERROR: !!! MOTOR ALARM !!! MOTOR ALARM !!!",eError);
|
|
sprintf(pBueffel,
|
|
"ERROR: too many position errors counted at motor %s",
|
|
self->name);
|
|
ServerWriteGlobal(pBueffel,eError);
|
|
SCSetInterrupt(pCon,eAbortBatch);
|
|
self->pDrivInt->iErrorCount = 0;
|
|
return 0;
|
|
}
|
|
|
|
/* Boundaries OK, send command */
|
|
self->fTarget = fHard;
|
|
iRet = self->pDriver->RunTo(self->pDriver,fHard);
|
|
if(iRet != OKOK)
|
|
{ /* try three times to fix it */
|
|
for(i = 0; (i < 3) && (iRet != OKOK); i++)
|
|
{
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
|
|
sprintf(pBueffel,"WARNING: Trying to fix: %s",pError);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, fHard);
|
|
switch(iRet)
|
|
{
|
|
case MOTFAIL:
|
|
SCWrite(pCon,pError,eError);
|
|
SCWrite(pCon,"\n",eError);
|
|
SCSetInterrupt(pCon,(int)ObVal(self->ParArray,INT));
|
|
return HWFault;
|
|
case MOTREDO:
|
|
iRet = self->pDriver->RunTo(self->pDriver,fHard);
|
|
if(iRet == OKOK)
|
|
{
|
|
return OKOK;
|
|
}
|
|
break;
|
|
case MOTOK:
|
|
return OKOK;
|
|
break;
|
|
}
|
|
}
|
|
/* tried three times, refuses to work */
|
|
SCWrite(pCon,pError,eError);
|
|
SCWrite(pCon,"\n",eError);
|
|
SCSetInterrupt(pCon,(int)ObVal(self->ParArray,INT));
|
|
return HWFault;
|
|
}
|
|
return OKOK;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
int MotorGetHardPosition(pMotor self,SConnection *pCon, float *fHard)
|
|
{
|
|
int iRet, iCode;
|
|
float fVal;
|
|
char pBueffel[512],pError[256];
|
|
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
iRet = self->pDriver->GetPosition(self->pDriver,&fVal);
|
|
if(iRet == OKOK) /* all went well, the exception */
|
|
{
|
|
*fHard = fVal;
|
|
return 1;
|
|
}
|
|
else /* a problem, the usual case: try fix the problem */
|
|
{ /* no point in trying this three times */
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,255);
|
|
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, fVal);
|
|
sprintf(pBueffel,"WARNING: Trying to fix %s",pError);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
switch(iRet)
|
|
{
|
|
case MOTFAIL:
|
|
sprintf(pBueffel,"ERROR: cannot fix motor %s",
|
|
self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
SCSetInterrupt(pCon,(int)ObVal(self->ParArray,INT));
|
|
*fHard = fVal;
|
|
return 0;
|
|
case MOTOK:
|
|
case MOTREDO:
|
|
iRet = self->pDriver->GetPosition(self->pDriver,&fVal);
|
|
if(iRet)
|
|
{
|
|
*fHard = fVal*ObVal(self->ParArray,SIGN);
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
sprintf(pBueffel,"ERROR: cannot fix motor %s",
|
|
self->name);
|
|
SCSetInterrupt(pCon,(int)ObVal(self->ParArray,INT));
|
|
SCWrite(pCon,pBueffel,eError);
|
|
*fHard = fVal;
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
*fHard = fVal*ObVal(self->ParArray,SIGN);
|
|
return 0;
|
|
}
|
|
/* ------------------------------------------------------------------------*/
|
|
int MotorGetSoftPosition(pMotor self, SConnection *pCon, float *fVal)
|
|
{
|
|
int iRet;
|
|
float fValue;
|
|
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
/* get the hard position */
|
|
iRet = MotorGetHardPosition(self,pCon,&fValue);
|
|
if(!iRet)
|
|
{
|
|
*fVal = fValue;
|
|
return 0;
|
|
}
|
|
|
|
/* apply zeropoint */
|
|
if(ObVal(self->ParArray,SIGN) < 0.)
|
|
{
|
|
fValue += ObVal(self->ParArray,SZERO);
|
|
}
|
|
else
|
|
{
|
|
fValue -= ObVal(self->ParArray,SZERO);
|
|
}
|
|
*fVal = fValue;
|
|
|
|
/* apply sign */
|
|
*fVal = fValue*ObVal(self->ParArray,SIGN);
|
|
return 1;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
int MotorCheckPosition(void *sulf, SConnection *pCon)
|
|
{
|
|
float fHard;
|
|
int i, iRet, iCode;
|
|
char pBueffel[512];
|
|
pMotor self;
|
|
|
|
self = (pMotor)sulf;
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
/* try to find current position */
|
|
iRet = MotorGetHardPosition(self,pCon,&fHard);
|
|
if(iRet)
|
|
{
|
|
if(absf(fHard - self->fTarget) < ObVal(self->ParArray,PREC))
|
|
{
|
|
self->fPosition = fHard;
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
/* Oooopppsss error */
|
|
return 0;
|
|
}
|
|
}
|
|
else /* error getting hold of position, MotorGetHard already tried to
|
|
solve the problem and FAILED, the fucking bastard
|
|
even messaged the connection accordingly */
|
|
{
|
|
return -1;
|
|
}
|
|
}
|
|
/* --------------------------------------------------------------------------
|
|
The Factory function for creating a motor. Usage:
|
|
MotorCreate name DriverName.
|
|
|
|
Drivernames currently understood are:
|
|
EL734 SINQ EL734 stepper motor
|
|
EL734DC SINQ EL734 DC motor
|
|
PiPiezo Physik-Instrumente E-255 Piezo Controller
|
|
SIM simulated motor
|
|
*/
|
|
|
|
extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
|
|
|
|
int MotorCreate(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pMotor pNew = NULL;
|
|
MotorDriver *pDriver = NULL;
|
|
char pBueffel[512];
|
|
int iD, iRet;
|
|
Tcl_Interp *pTcl = (Tcl_Interp *)pSics->pTcl;
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
|
|
/* a first check */
|
|
if(argc < 3)
|
|
{
|
|
SCWrite(pCon,"Insufficient arguments for motor creation",eError);
|
|
return 0;
|
|
}
|
|
|
|
/* create the driver */
|
|
strtolower(argv[2]);
|
|
strtolower(argv[1]);
|
|
if(strcmp(argv[2],"el734") == 0)
|
|
{
|
|
iD = argc - 3;
|
|
pDriver = CreateEL734(pCon,iD,&argv[3]);
|
|
if(!pDriver)
|
|
{
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("EL734",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
else if(strcmp(argv[2],"ecb") == 0)
|
|
{
|
|
iD = argc - 3;
|
|
pDriver = CreateECBMotor(pCon,iD,&argv[3]);
|
|
if(!pDriver)
|
|
{
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("ECB",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
else if(strcmp(argv[2],"el734dc") == 0)
|
|
{
|
|
iD = argc - 3;
|
|
pDriver = CreateEL734DC(pCon,iD,&argv[3]);
|
|
if(!pDriver)
|
|
{
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("EL734DC",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
else if (strcmp(argv[2],"sim") == 0)
|
|
{
|
|
iD = argc - 3;
|
|
pDriver = CreateSIM(pCon,iD,&argv[3]);
|
|
if(!pDriver)
|
|
{
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("SIM",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
pNew->ParArray[HLOW].iCode = usUser;
|
|
pNew->ParArray[HUPP].iCode = usUser;
|
|
}
|
|
else if (strcmp(argv[2],"pipiezo") == 0)
|
|
{
|
|
pDriver = MakePiPiezo(pSics->pTcl,argv[3]);
|
|
if(!pDriver)
|
|
{
|
|
|
|
SCWrite(pCon,pTcl->result,eError);
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("PIPIEZO",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
sprintf(pBueffel,"Motor Type %s not recognized for motor %s",
|
|
argv[2],argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
|
|
/* create the interpreter command */
|
|
iRet = AddCommand(pSics,argv[1],MotorAction,MotorKill,pNew);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pBueffel,"ERROR: duplicate command %s not created",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/* ----------------- some private functions used in MotorAction -------------*/
|
|
void MotorListLL(pMotor self, SConnection *pCon)
|
|
{
|
|
char pBueffel[512];
|
|
int i, iLen;
|
|
|
|
iLen = ObParLength(self->ParArray);
|
|
sprintf(pBueffel,"Parameter Listing for motor %s\n",self->name);
|
|
SCWrite(pCon,pBueffel,eStatus);
|
|
for(i = 0; i < iLen; i++)
|
|
{
|
|
sprintf(pBueffel,"%s.%s = %f\n",self->name,
|
|
self->ParArray[i].name,self->ParArray[i].fVal);
|
|
SCWrite(pCon,pBueffel,eStatus);
|
|
}
|
|
sprintf(pBueffel,"%s.Position = %f\n", self->name,self->fPosition);
|
|
SCWrite(pCon,pBueffel,eStatus);
|
|
sprintf(pBueffel,"%s.TargetPosition = %f\n", self->name,self->fTarget);
|
|
SCWrite(pCon,pBueffel,eStatus);
|
|
|
|
/*
|
|
list driver parameters when appropriate
|
|
*/
|
|
if(self->pDriver->ListDriverPar != NULL)
|
|
{
|
|
self->pDriver->ListDriverPar(self->pDriver,self->name, pCon);
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
void MotorReset(pMotor pM)
|
|
{
|
|
ObParInit(pM->ParArray,SLOW,"softlowerlim",pM->pDriver->fLower,usUser);
|
|
ObParInit(pM->ParArray,SUPP,"softupperlim",pM->pDriver->fUpper,usUser);
|
|
ObParInit(pM->ParArray,SZERO,"softzero",ZEROINACTIVE,usUser);
|
|
ObParInit(pM->ParArray,FIX,"fixed",-1,usUser);
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
typedef struct {
|
|
char *pName;
|
|
SConnection *pCon;
|
|
} MotInfo, *pMotInfo;
|
|
/*-----------------------------------------------------------------------*/
|
|
static void KillInfo(void *pData)
|
|
{
|
|
pMotInfo self = NULL;
|
|
|
|
assert(pData);
|
|
self = (pMotInfo)pData;
|
|
if(self->pName)
|
|
{
|
|
free(self->pName);
|
|
}
|
|
free(self);
|
|
}
|
|
/*------------------- The CallBack function for interest ------------------*/
|
|
static int InterestCallback(int iEvent, void *pEvent, void *pUser)
|
|
{
|
|
pMotInfo pInfo = NULL;
|
|
char pBueffel[80];
|
|
MotCallback *psCall;
|
|
|
|
assert(pEvent);
|
|
assert(pUser);
|
|
|
|
psCall = (MotCallback *)pEvent;
|
|
pInfo = (MotInfo *)pUser;
|
|
|
|
sprintf(pBueffel,"%s.position = %f ", pInfo->pName, psCall->fVal);
|
|
SCWrite(pInfo->pCon,pBueffel,eValue);
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static void KillScript(void *pData)
|
|
{
|
|
if(pData != NULL)
|
|
{
|
|
free(pData);
|
|
}
|
|
}
|
|
/*------------------------ The endscript callback function ----------------*/
|
|
static int EndScriptCallback(int iEvent, void *pEvent, void *pUser)
|
|
{
|
|
char *pScript = NULL;
|
|
MotCallback *psCall;
|
|
char pCommand[1024];
|
|
int iRet;
|
|
|
|
assert(pEvent);
|
|
assert(pUser);
|
|
|
|
psCall = (MotCallback *)pEvent;
|
|
pScript = (char *)pUser;
|
|
|
|
sprintf(pCommand,"%s %f",pScript,psCall->fVal);
|
|
iRet = Tcl_Eval(pServ->pSics->pTcl,pCommand);
|
|
return iRet;
|
|
}
|
|
/*----------------------------------------------------------------------------
|
|
The wrapper function for a motor. Commands currently supported are:
|
|
|
|
motorname parametername newval : sets a parameter
|
|
motorname parametername : gets the value for a par
|
|
motorname list : lists all parameters
|
|
motorname reset : puts softlimits to default
|
|
motorname interest : starts sending automatic
|
|
notifications when driving
|
|
motorname endscript : script to call when motor
|
|
finishes driving
|
|
-----------------------------------------------------------------------------*/
|
|
int MotorAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
char pBueffel[512];
|
|
TokenList *pList = NULL;
|
|
TokenList *pCurrent;
|
|
TokenList *pName;
|
|
int iRet;
|
|
pMotor self;
|
|
float fValue;
|
|
long lID;
|
|
pMotInfo pMoti = NULL;
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
assert(pData);
|
|
|
|
self = (pMotor)pData;
|
|
|
|
/* create Tokenlist */
|
|
argtolower(argc,argv);
|
|
pList = SplitArguments(argc,argv);
|
|
if(!pList)
|
|
{
|
|
SCWrite(pCon,"Error parsing argument list in MotorAction",eError);
|
|
return 0;
|
|
}
|
|
|
|
/* first argument can be one of list, reset or parameter name */
|
|
pCurrent = pList->pNext;
|
|
if(!pCurrent) /* no argument, print value */
|
|
{
|
|
iRet = MotorGetSoftPosition(self,pCon,&fValue);
|
|
if(!iRet)
|
|
{
|
|
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,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);
|
|
DeleteTokenList(pList);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
else if(strcmp(pCurrent->text,"interest") == 0) /* interest */
|
|
{
|
|
pMoti = (pMotInfo)malloc(sizeof(MotInfo));
|
|
if(!pMoti)
|
|
{
|
|
SCWrite(pCon,"ERROR: out of memory in motor.c",eError);
|
|
return 0;
|
|
}
|
|
pMoti->pName = strdup(argv[0]);
|
|
pMoti->pCon = pCon;
|
|
lID = RegisterCallback(self->pCall, MOTDRIVE, InterestCallback,
|
|
pMoti, KillInfo);
|
|
SCRegister(pCon,pSics, self->pCall,lID);
|
|
DeleteTokenList(pList);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
else if(strcmp(pCurrent->text,"uninterest") == 0)
|
|
{
|
|
RemoveCallback2(self->pCall,pCon);
|
|
SCSendOK(pCon);
|
|
DeleteTokenList(pList);
|
|
return 1;
|
|
}
|
|
else if(strcmp(pCurrent->text,"endscript") == 0) /* endscript */
|
|
{
|
|
if(!SCMatchRights(pCon,usMugger))
|
|
{
|
|
return 0;
|
|
}
|
|
if(self->endScriptID != 0)
|
|
{
|
|
RemoveCallback(self->pCall, self->endScriptID);
|
|
self->endScriptID = 0;
|
|
}
|
|
pCurrent = pCurrent->pNext;
|
|
if(!pCurrent)
|
|
{
|
|
SCWrite(pCon,"ERROR: scriptname argument missing",eError);
|
|
return 0;
|
|
}
|
|
self->endScriptID =
|
|
RegisterCallback(self->pCall, MOTEND, EndScriptCallback,
|
|
strdup(pCurrent->text), KillScript);
|
|
SCRegister(pCon,pSics, self->pCall,self->endScriptID);
|
|
DeleteTokenList(pList);
|
|
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)
|
|
{
|
|
iRet = MotorGetSoftPosition(self,pCon,&fValue);
|
|
if(!iRet)
|
|
{
|
|
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;
|
|
}
|
|
else if(strcmp(pName->text,"hardposition") == 0)
|
|
{
|
|
iRet = MotorGetHardPosition(self,pCon,&fValue);
|
|
if(!iRet) {
|
|
sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
DeleteTokenList(pList);
|
|
return 0;
|
|
}
|
|
sprintf(pBueffel,"%s.HardPosition = %f",argv[0],fValue);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
DeleteTokenList(pList);
|
|
return 1;
|
|
}
|
|
iRet = MotorGetPar(self,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",self->name,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,pCon,pName->text,fValue);
|
|
DeleteTokenList(pList);
|
|
if(iRet)
|
|
SCSendOK(pCon);
|
|
return iRet;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|