linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
3cb901b43794d697485a289f935eb2d0be461af2
sics/velo.c
koennecke a5c2da6acf - Switched motor to hdb 
- Changes to Hipadaba
- Added project to histogram memory code
- Started regression testing code
- Added hill climbing as optimization method to optimise
2006-08-16 14:13:05 +00:00

1256 lines
36 KiB
C
Raw Blame History

/*---------------------------------------------------------------------------
V E L O C I T Y S E L E C T O R
The velocity selector module for SICS. For documentation see velo.w
and velo.tex.
Author: Mark Koennecke, June 1997
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 <string.h>
#include <stdlib.h>
#include <assert.h>
#include <ctype.h>
#include <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "lld.h"
#include "status.h"
#include "stringdict.h"
#include "splitter.h"
#include "obpar.h"
#include "motor.h"
#include "evcontroller.h"
#include "velo.h"
#include "velo.i"
#include "velodriv.h"
#include "site.h"
/* -------- defines for parameters to velocity selector */
#define INT 0
#define RIGHT 1
#define TILTPREC 2
/* ------------------ the functions for the drivable interface --------------*/
static int VSHalt(void *pData)
{
pVelSel self = NULL;
self = (pVelSel)pData;
assert(self);
self->pDriv->Halt(self->pDriv);
EVCSetMode(self->pMonitor,EVIdle);
return 1;
}
/*---------------------------------------------------------------------------*/
static int VSLimits(void *pData, float fVal, char *pError, int iErrLen)
{
pVelSel self = NULL;
Verbot Alcatraz;
int iRet;
char pBueffel[512];
self = (pVelSel)pData;
assert(self);
/* first let us check if we are in the overall range.
The first entry in the forbidden region thing is meant to
contain this information
*/
iRet = LLDnodePtr2First(self->iForbidden);
LLDnodeDataTo(self->iForbidden,&Alcatraz);
if( (fVal < Alcatraz.fMin) || (fVal > Alcatraz.fMax) )
{
sprintf(pBueffel," %f out of range: %f --- %f",
fVal, Alcatraz.fMin, Alcatraz.fMax);
strncpy(pError,pBueffel,iErrLen);
return 0;
}
/* now search through the rest if in some forbidden region */
iRet = LLDnodePtr2Next(self->iForbidden);
while(iRet != 0)
{
LLDnodeDataTo(self->iForbidden,&Alcatraz);
if( (fVal > Alcatraz.fMin) && (fVal < Alcatraz.fMax))
{
sprintf(pBueffel," %f violates forbidden region %f --- %f",
fVal, Alcatraz.fMin, Alcatraz.fMax);
strncpy(pError,pBueffel,iErrLen);
return 0;
}
iRet = LLDnodePtr2Next(self->iForbidden);
}
/* Success ! */
return 1;
}
/*-----------------------------------------------------------------------*/
static void VSListForbidden(pVelSel self, SConnection *pCon){
Tcl_DString message;
int status;
Verbot Alcatraz;
char pBueffel[256];
Tcl_DStringInit(&message);
/*
The first entry in the forbidden region thing is meant to
contain the overall range: skip it!
*/
status = LLDnodePtr2First(self->iForbidden);
LLDnodeDataTo(self->iForbidden,&Alcatraz);
if(status == 1){
snprintf(pBueffel,255,"%s.forbidden = {%f -inf", self->pName, Alcatraz.fMax);
Tcl_DStringAppend(&message,pBueffel,strlen(pBueffel));
}
/* now search the forbidden regions */
status = LLDnodePtr2Next(self->iForbidden);
while(status != 0){
LLDnodeDataTo(self->iForbidden,&Alcatraz);
snprintf(pBueffel,255,", %f - %f", Alcatraz.fMin, Alcatraz.fMax);
Tcl_DStringAppend(&message,pBueffel,strlen(pBueffel));
status = LLDnodePtr2Next(self->iForbidden);
}
Tcl_DStringAppend(&message,"}",1);
SCWrite(pCon,Tcl_DStringValue(&message),eValue);
Tcl_DStringFree(&message);
}
/*-------------------------------------------------------------------------*/
static long VSSetValue(void *pData,SConnection *pCon, float fVal)
{
pVelSel self = NULL;
int i, iRet, iCode, iTest;
char pError[132], pBueffel[512];
self = (pVelSel)pData;
assert(self);
/* check user rights */
iRet = SCMatchRights(pCon,(int)ObVal(self->pPar,RIGHT));
if(iRet != 1 )
{
SCWrite(pCon,"ERROR: you are not authorised to drive velocity selector",
eError);
return 0;
}
/* try mimimum three times to get there */
for(i = 0; i < 3; i++)
{
iRet = self->pDriv->SetRotation(self->pDriv,fVal);
if(iRet)
{
EVCSetMode(self->pMonitor,EVDrive);
return 1;
}
else /* error case */
{
self->pDriv->GetError(self->pDriv, &iCode, pError, 131);
sprintf(pBueffel,"WARNING: trying to fix: %s",
pError);
SCWrite(pCon,pBueffel,eStatus);
iTest = self->pDriv->TryAndFixIt(self->pDriv,iCode);
switch(iTest)
{
case VELOOK:
EVCSetMode(self->pMonitor,EVDrive);
return 1;
break;
case VELOREDO:
/* actual redo done by loop */
break;
case VELOFAIL:
sprintf(pBueffel,"ERROR: aborting with %s", pError);
SCWrite(pCon,pBueffel,eError);
SCSetInterrupt(pCon,(int)ObVal(self->pPar,INT));
return 0;
default:
assert(0);
}
}
}
/* if we are here we tried three times and failed to get it
work
*/
sprintf(pBueffel,
"ERRROR: Failed 3 times to vary rotation speed. Failing.");
SCWrite(pCon,pBueffel,eError);
SCSetInterrupt(pCon,(int)ObVal(self->pPar,INT));
EVCSetMode(self->pMonitor,EVIdle);
return 0;
}
/*-----------------------------------------------------------------------*/
static int VSCheckStatus(void *pData, SConnection *pCon)
{
pVelSel self = NULL;
int iRet, iCode,iTest;
char pError[132], pBueffel[512];
int iSig;
float fVal;
self = (pVelSel)pData;
assert(self);
iRet = self->pDriv->GetStatus(self->pDriv,&iSig,&fVal);
InvokeCallBack(self->pCall,iSig,&fVal);
if(SCGetInterrupt(pCon) != eContinue)
{
return HWFault;
}
switch(iRet)
{
case VSACCEL:
return HWBusy;
break;
case VSOK:
EVCSetMode(self->pMonitor,EVMonitor);
return OKOK;
break;
case VSNOCON:
case VSFAIL:
self->pDriv->GetError(self->pDriv,&iCode,pError,131);
sprintf(pBueffel,"WARNING: %s detected",pError);
SCWrite(pCon,pBueffel,eWarning);
iTest = self->pDriv->TryAndFixIt(self->pDriv,iCode);
switch(iTest)
{
case VELOOK:
case VELOREDO:
return HWBusy;
break;
case VELOFAIL:
SCWrite(pCon,
"ERROR: velocity selector can not fix problem",
eError);
SCSetInterrupt(pCon,(int)ObVal(self->pPar,INT));
EVCSetMode(self->pMonitor,EVIdle);
return HWFault;
default:
assert(0);
}
default:
assert(0);
}
/* not reached */
return HWFault;
}
/*---------------------------------------------------------------------------*/
static float VSGetValue(void *pData, SConnection *pCon)
{
pVelSel self = NULL;
int iRet, iCode,i, iTest;
char pError[132], pBueffel[512];
float fVal;
self = (pVelSel)pData;
assert(self);
/* try three times, maximum */
for(i = 0; i < 3; i++)
{
iRet = self->pDriv->GetRotation(self->pDriv,&fVal);
if(!iRet)
{
self->pDriv->GetError(self->pDriv,&iCode,pError,131);
iTest = self->pDriv->TryAndFixIt(self->pDriv,iCode);
switch(iTest)
{
case VELOOK:
case VELOREDO:
sprintf(pBueffel,"WARNING: problem %s fixed",pError);
SCWrite(pCon,pBueffel,eStatus);
break;
case VELOFAIL:
sprintf(pBueffel,"ERROR: %s", pError);
SCWrite(pCon,pBueffel,eError);
return -9999.;
default:
assert(0);
}
}
else
{
return fVal;
}
}
return -9999;
}
/*-------------------------------------------------------------------------*/
static void *VSGetInterface(void *pData, int iID)
{
pVelSel self = NULL;
self = (pVelSel)pData;
assert(self);
if(iID == DRIVEID)
{
return self->pDrivInt;
}
return NULL;
}
/*--------------------------------------------------------------------------*/
pVelSel VSCreate(pMotor pTilt, pVelSelDriv pDriv)
{
pVelSel pNew = NULL;
SConnection *pCon = NULL;
assert(pTilt);
assert(pDriv);
/* allocate memory */
pNew = (pVelSel)malloc(sizeof(VelSel));
if(!pNew)
{
return NULL;
}
memset(pNew,0,sizeof(VelSel));
/* make descriptor */
pNew->pDes = CreateDescriptor("VelocitySelector");
if(!pNew->pDes)
{
free(pNew);
return NULL;
}
pNew->pDes->GetInterface = VSGetInterface;
/* create list */
pNew->iForbidden = LLDcreate(sizeof(Verbot));
if(pNew->iForbidden < 0)
{
VSDestroy(pNew);
return NULL;
}
/* the parameter array */
pNew->pPar = ObParCreate(3);
if(!pNew->pPar)
{
VSDestroy(pNew);
return NULL;
}
ObParInit(pNew->pPar,INT,"interrupt",(float)eAbortBatch,usMugger);
ObParInit(pNew->pPar,RIGHT,"userrights",(float)usUser,usMugger);
ObParInit(pNew->pPar,TILTPREC,"tilttolerance",0.1,usMugger);
/* the driveable interface */
pNew->pDrivInt = CreateDrivableInterface();
if(!pNew->pDrivInt)
{
VSDestroy(pNew);
return NULL;
}
pNew->pDrivInt->Halt = VSHalt;
pNew->pDrivInt->CheckLimits = VSLimits;
pNew->pDrivInt->SetValue = VSSetValue;
pNew->pDrivInt->CheckStatus = VSCheckStatus;
pNew->pDrivInt->GetValue = VSGetValue;
/* The callback interface */
pNew->pCall = CreateCallBackInterface();
if(!pNew->pCall)
{
VSDestroy(pNew);
return 0;
}
/* deal with that motor, have him AccessCode Internal */
pNew->pTilt = pTilt;
pCon = SCCreateDummyConnection(pServ->pSics);
if(pCon != NULL)
{
MotorSetPar(pTilt,pCon,"accesscode",(float)usInternal);
SCDeleteConnection(pCon);
}
/* enter driver */
pNew->pDriv = pDriv;
pNew->pDriv->fTolerance = 15.;
return pNew;
}
/*-------------------------------------------------------------------------*/
void VSDestroy(void *pData)
{
char pBueffel[132];
pVelSel self = NULL;
self = (pVelSel)pData;
assert(self);
if(self->pDes)
{
DeleteDescriptor(self->pDes);
}
if(self->pPar)
{
ObParDelete(self->pPar);
}
if(self->pDriv)
{
VSDeleteDriver(self->pDriv);
}
if(self->pCall)
{
DeleteCallBackInterface(self->pCall);
}
LLDdelete(self->iForbidden);
if(self->pDrivInt)
{
free(self->pDrivInt);
}
if(self->pMonitor)
{
strtolower(self->pName);
sprintf(pBueffel,"%swatch",self->pName);
/* EVUnregister(FindEMON(pServ->pSics),pBueffel); */
DeleteEVController(self->pMonitor);
}
if(self->pName)
free(self->pName);
free(self);
}
/*--------------------------------------------------------------------------*/
int VSAddVerbot(pVelSel self, float fMin, float fMax)
{
Verbot Alcatraz;
assert(self);
Alcatraz.fMin = fMin;
Alcatraz.fMax = fMax;
LLDnodeAppendFrom(self->iForbidden,&Alcatraz);
return 1;
}
/*-------------------------------------------------------------------------*/
int VSGetPar(pVelSel self, char *name, float *fVal)
{
ObPar *pPar = NULL;
assert(self);
pPar = ObParFind(self->pPar,name);
if(pPar)
{
*fVal = pPar->fVal;
return 1;
}
return 0;
}
/*-------------------------------------------------------------------------*/
int VSSetPar(pVelSel self,SConnection *pCon, char *name, float fVal)
{
assert(self);
return ObParSet(self->pPar,self->pDes->name, name, fVal,pCon);
}
/*------------------------------------------------------------------------*/
int VSGetRotation(pVelSel self, float *fRot)
{
assert(self);
return self->pDriv->GetRotation(self->pDriv,fRot);
}
/*------------------------------------------------------------------------*/
int VSGetTilt(pVelSel self, float *fTilt)
{
assert(self);
return self->pTilt->pDriver->GetPosition(self->pTilt->pDriver, fTilt);
}
/*------------------------------------------------------------------------*/
int VSSetTiltRot(pVelSel self, SConnection *pCon, float fNewRot,
float fNewTilt)
{
float fDelta, fOldTilt;
int iRet;
int iOldRight;
char pError[132];
char pBueffel[256];
assert(self);
/* check if rotation in limits */
iRet = VSLimits(self,fNewRot,pError,131);
if(!iRet)
{
sprintf(pBueffel,"ERROR: %s",pError);
SCWrite(pCon,pBueffel,eError);
return 0;
}
/* second: do I really need to modify tilt? */
iRet = MotorGetSoftPosition(self->pTilt, pCon,&fOldTilt);
if(!iRet)
{
return 0;
}
fDelta = fNewTilt - fOldTilt;
if(fDelta < 0)fDelta = -fDelta;
if(fDelta > ObVal(self->pPar,TILTPREC)) /* yes */
{
/* first stop the Rotation of the selector */
iRet = StartDevice(GetExecutor(),"Velocity Selector Rot",
self->pDes, self, pCon,-10.);
if(!iRet)
{
return 0;
}
/* wait for this to finish */
SCWrite(pCon,"Stopping Velocity Selector, this may take a long while",eWarning);
iRet = Wait4Success(GetExecutor());
if(iRet == DEVINT)
{
if(SCGetInterrupt(pCon) == eAbortOperation)
{
SCSetInterrupt(pCon,eContinue);
}
return 0;
}
/* OK, selector stoped, drive tilt motor. We have to give internal
privilege to us beforehand, though
*/
iOldRight = SCGetRights(pCon);
SCSetRights(pCon,usInternal);
iRet = StartDevice(GetExecutor(),"Velocity Selector Tilt",
self->pTilt->pDescriptor, self->pTilt,
pCon,fNewTilt);
/* wait for this to finish */
SCWrite(pCon,"Driving tilt-angle",eWarning);
iRet = Wait4Success(GetExecutor());
if(iRet == DEVINT)
{
if(SCGetInterrupt(pCon) == eAbortOperation)
{
SCSetInterrupt(pCon,eContinue);
}
return 0;
}
/* reset userrights */
SCSetRights(pCon,iOldRight);
}
/* drive rotation */
iRet = StartDevice(GetExecutor(),"Velocity Selector Rot",
self->pDes, self, pCon,fNewRot);
if(!iRet)
{
return 0;
}
/* wait for this to finish */
SCWrite(pCon,"Running velocity selector to speed, this may take ages",
eWarning);
iRet = Wait4Success(GetExecutor());
if(iRet == DEVINT)
{
if(SCGetInterrupt(pCon) == eAbortOperation)
{
SCSetInterrupt(pCon,eContinue);
}
return 0;
}
return 1;
}
/*--------------------------------------------------------------------------*/
int VSGetLossCurrent(pVelSel self, SConnection *pCon, float *fLoss)
{
int iRet, iCode;
char pError[132], pBueffel[512];
Status eOld;
assert(self);
assert(pCon);
eOld = GetStatus();
SetStatus(eRunning);
iRet = self->pDriv->GetLossCurrent(self->pDriv,fLoss);
SetStatus(eOld);
if(!iRet)
{
self->pDriv->GetError(self->pDriv,&iCode,pError,131);
sprintf(pBueffel,"ERROR: %s while trying to measure loss current",
pError);
SCWrite(pCon,pBueffel,eError);
return 0;
}
return 1;
}
/*=======================================================================
Functions and definitions for the dummy environment device driver
for the velocity selector. This has to be here, as it uses many static
functions already defined.
---------------------------------------------------------------------------*/
#define VELONOTPERMITTED -1122
#include "evdriver.i"
/*------------------ driver private data structure -----------------------*/
typedef struct {
pVelSel pSel;
int iLastError;
} *pVelPrivate, VelPrivate;
/*------------------------------------------------------------------------*/
static void KillVelPrivate(void *pData)
{
pVelPrivate self = NULL;
self = (pVelPrivate)pData;
if(self)
{
free(self);
}
}
/*------------------------------------------------------------------------*/
static int DummyVelSet(pEVDriver self, float fNew)
{
pVelPrivate ich = NULL;
assert(self);
ich = (pVelPrivate)self->pPrivate;
assert(ich);
/* not permitted, must be done through nvs */
ich->iLastError = VELONOTPERMITTED;
return 0;
}
/*-------------------------------------------------------------------------*/
static int DummyVelGet(pEVDriver self, float *fPos)
{
float fVal;
pVelPrivate ich = NULL;
assert(self);
ich = (pVelPrivate)self->pPrivate;
assert(ich);
ich->iLastError = 0;
return ich->pSel->pDriv->GetRotation(ich->pSel->pDriv,fPos);
}
/*-------------------------------------------------------------------------*/
static int DummyVelSend(pEVDriver self, char *pCommand,
char *pReply, int ReplyLen)
{
pVelPrivate ich = NULL;
assert(self);
ich = (pVelPrivate)self->pPrivate;
assert(ich);
/* not permitted, must be done through nvs */
ich->iLastError = VELONOTPERMITTED;
return 0;
}
/*-------------------------------------------------------------------------*/
static int DummyVelError(pEVDriver self, int *iCode, char *pError,
int iErrLen)
{
pVelPrivate ich = NULL;
assert(self);
ich = (pVelPrivate)self->pPrivate;
assert(ich);
if(ich->iLastError == VELONOTPERMITTED)
{
strncpy(pError,
"ERROR: this operation is NOT Permitted, use velocity selector object instead",
iErrLen);
*iCode = VELONOTPERMITTED;
return 1;
}
else
{
return ich->pSel->pDriv->GetError(ich->pSel->pDriv,iCode,
pError,iErrLen);
}
}
/*-------------------------------------------------------------------------*/
static int DummyVelFix(pEVDriver self, int iCode)
{
pVelPrivate ich = NULL;
assert(self);
ich = (pVelPrivate)self->pPrivate;
assert(ich);
if(ich->iLastError == VELONOTPERMITTED)
{
ich->iLastError = 0;
return DEVFAULT;
}
else
{
return ich->pSel->pDriv->TryAndFixIt(ich->pSel->pDriv,iCode);
}
}
/*-------------------------------------------------------------------------*/
static int DummyVelInit(pEVDriver self)
{
return 1;
}
/*-------------------------------------------------------------------------*/
pEVDriver MakeDummyVel(pVelSel pVel)
{
pEVDriver pNew = NULL;
pVelPrivate ich = NULL;
pNew = (pEVDriver)malloc(sizeof(EVDriver));
if(!pNew)
{
return NULL;
}
ich = (pVelPrivate)malloc(sizeof(VelPrivate));
if(!ich)
{
free(pNew);
return NULL;
}
memset(pNew,0,sizeof(EVDriver));
memset(ich,0,sizeof(VelPrivate));
/* initialise function pointers */
pNew->SetValue = DummyVelSet;
pNew->GetValue = DummyVelGet;
pNew->Send = DummyVelSend;
pNew->GetError = DummyVelError;
pNew->TryFixIt = DummyVelFix;
pNew->Init = DummyVelInit;
pNew->Close = DummyVelInit;
ich->pSel = pVel;
pNew->pPrivate = ich;
pNew->KillPrivate = KillVelPrivate;
return pNew;
}
/*---------------------------------------------------------------------------
Usage:
VelSelFactory name TiltMot DRIVER optionname
- name is the name of the thing in SICS
- TiltMot is the name of a motor driving the tilt
- DRIVER is the type of driver to use.
- TclArray is a Tcl array containing configuration
options for the driver.
*/
int VelSelFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[])
{
pVelSelDriv pDriv = NULL;
pVelSel pNew = NULL;
pMotor pTilt = NULL;
char pBueffel[256];
Tcl_Interp *pT = NULL;
int iRet;
pEVDriver pMonDriv = NULL;
pSite site = NULL;
assert(pCon);
assert(pSics);
/* minimum 4 arguments! */
if(argc < 4)
{
SCWrite(pCon,"ERROR: Insufficient number of arguments to VelSelFactory",
eError);
return 0;
}
/* first one is name */
/* second one should be motor */
strtolower(argv[1]);
strtolower(argv[2]);
pTilt = FindMotor(pSics,argv[2]);
if(!pTilt)
{
sprintf(pBueffel,"ERROR: in VelSelFactory --> %s is not valid motor!",
argv[2]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
/* third should be driver name */
strtolower(argv[3]);
if(strcmp(argv[3],"sim") == 0) /* Mr. Simulation */
{
pDriv = VSCreateSim();
}
else
{
if(argc < 5)
{
sprintf(pBueffel,
"ERROR: missing options array for velocity selector");
SCWrite(pCon,pBueffel,eError);
return 0;
}
pT = InterpGetTcl(pSics);
site = getSite();
if(site != NULL)
{
pDriv = site->CreateVelocitySelector(argv[3],argv[4],pT);
}
if(!pDriv)
{
SCWrite(pCon,pT->result,eError);
return 0;
}
}
if(!pDriv)
{
sprintf(pBueffel,"ERROR: creating velocity selector driver %s",
argv[3]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
/* now initialise this and install it as command */
pNew = VSCreate(pTilt,pDriv);
if(!pNew)
{
SCWrite(pCon,"ERROR: creating velocity selector, no memory",eError);
return 0;
}
iRet = pDriv->Init(pDriv,pCon);
if(!iRet)
{
SCWrite(pCon,"ERROR: failed to initialize velocity selector",
eError);
VSDestroy(pNew);
return 0;
}
pNew->pName = strdup(argv[1]);
iRet = AddCommand(pSics,argv[1],VelSelAction,
VSDestroy,pNew);
if(!iRet)
{
sprintf(pBueffel,"ERROR: duplicate command %s not created",argv[2]);
SCWrite(pCon,pBueffel,eError);
VSDestroy((void *)pNew);
return 0;
}
/* install the evcontroller bit of the velocity selector */
pMonDriv = MakeDummyVel(pNew);
if(!pMonDriv)
{
RemoveCommand(pSics,argv[1]);
SCWrite(pCon,"ERROR: failed to create monitor for nvs",eError);
return 0;
}
pBueffel[0] = '\0';
strcpy(pBueffel,argv[1]);
strcat(pBueffel,"watch");
pNew->pMonitor = CreateEVController(pMonDriv,pBueffel,&iRet);
if(!pNew->pMonitor)
{
DeleteEVDriver(pMonDriv); /* was missing M.Z. Jul 04 */
SCWrite(pCon,"ERROR: failed to create monitor for nvs",eError);
return 0;
}
iRet = AddCommand(pSics,pBueffel,EVControlWrapper,
NULL,pNew->pMonitor);
if(!iRet)
{
sprintf(pBueffel,"ERROR: duplicate command %s not created",pBueffel);
RemoveCommand(pSics,argv[1]);
return 0;
}
EVRegisterController(FindEMON(pSics),pBueffel,pNew->pMonitor,pCon);
return iRet;
}
/*------------------------------------------------------------------------*/
typedef struct {
SConnection *pCon;
char *pName;
} CBData;
/*------------------------------------------------------------------------*/
static void KillCB(void *pData)
{
CBData *pCB = NULL;
pCB = (CBData *)pData;
if(pCB)
{
if(pCB->pName)
{
free(pCB->pName);
}
free(pCB);
}
}
/*------------------------------------------------------------------------*/
static int RotationInterest(int iEvent, void *pData, void *pUser,
commandContext cc)
{
CBData *pDat = NULL;
float *fVal = NULL;
char pBueffel[512];
fVal = (float *)pData;
pDat = (CBData *)pUser;
assert(fVal);
assert(pData);
if(iEvent == ROTSTART)
{
sprintf(pBueffel,"%s Starting",pDat->pName);
SCWriteInContext(pDat->pCon,pBueffel,eWarning,cc);
}
else if(iEvent == ROTMOVE)
{
sprintf(pBueffel,"%s.rpm = %f",pDat->pName, *fVal);
SCWriteInContext(pDat->pCon,pBueffel,eWarning,cc);
}
return 1;
}
/*------------------------------------------------------------------------*/
int VelSelAction(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[])
{
pVelSel self = NULL;
char pCommand[512], pBueffel[256];
char *pPtr = NULL;
float fTilt, fRot, fVal;
int iDrive = 0;
int iRet;
float fMin, fMax, fLoss, fTol;
double dVal;
ObPar *pPar = NULL;
CBData *pCB = NULL;
long lID;
self = (pVelSel)pData;
assert(self);
assert(pCon);
assert(pSics);
if(argc < 2)
{
sprintf(pBueffel,"ERROR: %s expects at least one parameter",argv[0]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
/* check for parameters */
pPar = ObParFind(self->pPar,argv[1]);
if(pPar) /* yes it is a parameter */
{
if(argc >= 3) /* user wants to set */
{
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
if(iRet != TCL_OK)
{
sprintf(pBueffel,"ERROR: %s not recognized as numeric value for parameter",
argv[2]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
return ObParSet(self->pPar,argv[0], argv[1],(float)dVal,pCon);
}
else /* just print it */
{
sprintf(pBueffel,"%s.%s = %f",argv[0],argv[1],pPar->fVal);
SCWrite(pCon,pBueffel,eValue);
return 1;
}
}
/* rotinterest command */
if(strcmp(argv[1],"rotinterest") == 0)
{
pCB = (CBData *)malloc(sizeof(CBData));
if(!pCB)
{
SCWrite(pCon,"ERROR: no memory in velo.c ",eError);
return 0;
}
pCB->pCon = pCon;
pCB->pName = strdup(argv[0]);
lID = RegisterCallback(self->pCall, SCGetContext(pCon), ROTSTART, RotationInterest,
pCB, KillCB);
SCRegister(pCon,pSics, self->pCall,lID);
lID = RegisterCallback(self->pCall, SCGetContext(pCon), ROTMOVE, RotationInterest,
pCB, NULL);
SCRegister(pCon,pSics, self->pCall,lID);
SCSendOK(pCon);
return 1;
}
/* add command, adds forbidden regions */
if(strcmp(argv[1],"add") == 0)
{
/* only Managers may do this */
if(!SCMatchRights(pCon,usMugger))
{
SCWrite(pCon,"ERROR: you are NOT authorised to drive velocity selector",
eError);
return 0;
}
/* next two arguments must be min, max */
if( argc < 4)
{
sprintf(pBueffel,"ERROR: insufficient number to %s add",
argv[0]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
/* OK find fMin */
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
if(iRet != TCL_OK)
{
sprintf(pBueffel,"ERROR: %s not recognized as numeric value for fMin",
argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
fMin = (float)dVal;
/* OK find fMax */
iRet = Tcl_GetDouble(pSics->pTcl,argv[3],&dVal);
if(iRet != TCL_OK)
{
sprintf(pBueffel,"ERROR: %s not recognized as numeric value for fMax",
argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
fMax = (float)dVal;
/* do it */
VSAddVerbot(self,fMin,fMax);
SCSendOK(pCon);
return 1;
}
/* list forbidden regions */
if(strcmp(argv[1],"forbidden") == 0)
{
VSListForbidden(self,pCon);
return 1;
}
/* status, prints a status message */
if(strcmp(argv[1],"status") == 0)
{
self->pDriv->GetDriverText(self->pDriv,pBueffel,511);
SCWrite(pCon,pBueffel,eValue);
return 1;
}
/* loss command, determine loss current */
if(strcmp(argv[1],"loss") == 0 )
{
if(!SCMatchRights(pCon,usUser))
{
SCWrite(pCon,"ERROR: you are NOT authorised to determine loss current",
eError);
return 0;
}
iRet = VSGetLossCurrent(self,pCon,&fLoss);
if(iRet)
{
sprintf(pBueffel,"%s.LossCurrent = %f",argv[0], fLoss);
SCWrite(pCon,pBueffel,eValue);
return 1;
}
else
{ /* error should have been reported in VSGetLossCurrent */
return 0;
}
}
/* init command for reinitialising */
if(strcmp(argv[1],"init") == 0 )
{
if(!SCMatchRights(pCon,usUser))
{
SCWrite(pCon,"ERROR: you are NOT authorised to determine loss current",
eError);
return 0;
}
iRet = self->pDriv->Init(self->pDriv,pCon);
if(iRet)
{
SCSendOK(pCon);
return 1;
}
else
{ /* error should have been reported in Init */
return 0;
}
}
if(strcmp(argv[1],"rottolerance") == 0){
if(argc > 2){
/* set case */
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
if(iRet != TCL_OK){
sprintf(pBueffel,"ERROR: %s not recognized as numeric value for fMax",
argv[2]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
if(!SCMatchRights(pCon,usUser)){
return 0;
}
self->pDriv->fTolerance = dVal;
SCSendOK(pCon);
return 1;
} else {
sprintf(pBueffel,"%s.rottolerance = %f",
argv[0],self->pDriv->fTolerance);
SCWrite(pCon,pBueffel,eError);
return 1;
}
}
/* initialise the tilt and rot to current values */
iRet = VSGetRotation(self,&fRot);
if(!iRet)
{
SCWrite(pCon,"ERROR: cannot find current rotation speed",eError);
return 0;
}
iRet = MotorGetSoftPosition(self->pTilt,pCon,&fTilt);
if(!iRet)
{
SCWrite(pCon,"ERROR: failed to read tilt angle",eError);
return 0;
}
/* search for Tilt */
if(strcmp(argv[1],"tilt") == 0){
if(argc > 2){
/* set case */
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
if(iRet != TCL_OK){
sprintf(pBueffel,"ERROR: %s not recognized as numeric value for fMax",
argv[2]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
fTilt = dVal;
iDrive = 1;
} else {
sprintf(pBueffel,"%s tilt = %f",argv[0],fTilt);
SCWrite(pCon,pBueffel,eError);
return 1;
}
}
/* same for rot */
if(strcmp(argv[1],"rot") == 0){
if(argc > 2){
/* set case */
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
if(iRet != TCL_OK){
sprintf(pBueffel,"ERROR: %s not recognized as numeric value for fMax",
argv[2]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
fRot = dVal;
iDrive = 1;
} else {
sprintf(pBueffel,"%s rot = %f",argv[0],fRot);
SCWrite(pCon,pBueffel,eError);
return 1;
}
}
/* do drive if we really need to */
if(iDrive)
{
/* first check permission */
if(!SCMatchRights(pCon,ObVal(self->pPar,RIGHT)) )
{
SCWrite(pCon,"ERROR: you are NOT authorised to drive velocity selector",
eError);
return 0;
}
SetStatus(eDriving);
iRet = VSSetTiltRot(self,pCon,fRot,fTilt);
SetStatus(eEager);
if(iRet)
{
SCSendOK(pCon);
return 1;
}
else
{
return 0;
}
}
/* no driving asked for, check for other commands */
if(argc < 2)
{
goto end;
}
/* list command */
if(strcmp(argv[1],"list") == 0)
{
sprintf(pBueffel,"%s.rotation = %f\n%s.Tilt = %f",argv[0], fRot,
argv[0],fTilt);
SCWrite(pCon,pBueffel,eValue);
return 1;
}
end:
/* command not recognized */
sprintf(pBueffel,"ERROR: command %s not recognized",pCommand);
SCWrite(pCon,pBueffel,eError);
return 0;
}
Reference in New Issue View Git Blame Copy Permalink