- New batch file management module
- New oscillator module - Bug fixes SKIPPED: psi/buffer.c psi/el734hp.c psi/el737hpdriv.c psi/make_gen psi/nextrics.c psi/nxamor.c psi/pimotor.c psi/polterwrite.c psi/psi.c psi/swmotor2.c psi/tasscan.c psi/tricssupport.c psi/tricssupport.h psi/tecs/make_gen psi/utils/ecb_load_new/ecb_load.c psi/utils/ecb_load_new/ecb_load.h psi/utils/ecb_load_new/ecbdriv_els.c psi/utils/ecb_load_new/gpib_els.c psi/utils/ecb_load_new/makefile psi/utils/ecb_load_new/makefile_EGPIB psi/utils/ecb_load_new/makefile_GPIB
This commit is contained in:
cvs
467
confvirtualmot.c
Normal file
467
confvirtualmot.c
Normal file
@ -0,0 +1,467 @@
|
||||
/*-----------------------------------------------------------------------
|
||||
A configurable virtual motor object. At motor start a script is called
|
||||
which returns the motors and positions to drive as a comma separated
|
||||
list holding entries of the form mot=value.
|
||||
|
||||
This is the implementation file.
|
||||
|
||||
COPYRIGHT: see file COPYRIGHT
|
||||
|
||||
Mark Koennecke, August 2004
|
||||
-------------------------------------------------------------------------*/
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
#include "fortify.h"
|
||||
#include <tcl.h>
|
||||
#include "lld.h"
|
||||
#include "splitter.h"
|
||||
#include "confvirtmot.h"
|
||||
#include "confvirtmot.i"
|
||||
|
||||
extern char *stptok(char *s, char *t, int len, char *brk);
|
||||
|
||||
/*---------------------------------------------------------------------------
|
||||
An internal data structure which holds information required to control
|
||||
the real motors
|
||||
----------------------------------------------------------------------------*/
|
||||
typedef struct {
|
||||
char name[132];
|
||||
void *data;
|
||||
pIDrivable pDriv;
|
||||
float value;
|
||||
int running;
|
||||
} RealMotor, *pRealMotor;
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int HaltMotors(void *pData){
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
||||
RealMotor tuktuk;
|
||||
int iRet;
|
||||
|
||||
iRet = LLDnodePtr2First(self->motorList);
|
||||
while(iRet != 0)
|
||||
{
|
||||
LLDnodeDataTo(self->motorList,&tuktuk);
|
||||
tuktuk.pDriv->Halt(tuktuk.data);
|
||||
iRet = LLDnodePtr2Next(self->motorList);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static char *invokeMotorScript(pConfigurableVirtualMotor self,
|
||||
float newValue){
|
||||
char pBueffel[512];
|
||||
Tcl_Interp *pTcl;
|
||||
int status;
|
||||
|
||||
self->parseOK = 1;
|
||||
if(self->scriptName == NULL){
|
||||
snprintf(self->scriptError,511,
|
||||
"ERROR: no script configured for configurable virtual motor");
|
||||
self->parseOK = 0;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
snprintf(pBueffel,510,"%s %f",self->scriptName, newValue);
|
||||
pTcl = InterpGetTcl(pServ->pSics);
|
||||
|
||||
status = Tcl_Eval(pTcl, pBueffel);
|
||||
if(status != TCL_OK){
|
||||
snprintf(self->scriptError,510,"ERROR: Tcl subsystem reported %s",
|
||||
Tcl_GetStringResult(pTcl));
|
||||
self->parseOK = 0;
|
||||
return NULL;
|
||||
}
|
||||
return Tcl_GetStringResult(pTcl);
|
||||
}
|
||||
|
||||
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int parseEntry(pConfigurableVirtualMotor self,
|
||||
char *entry){
|
||||
RealMotor tuktuk;
|
||||
char *pPtr = NULL;
|
||||
char number[80], pError[256];
|
||||
CommandList *pCom = NULL;
|
||||
int status;
|
||||
|
||||
pPtr = entry;
|
||||
pPtr = stptok(pPtr,tuktuk.name,131,"=");
|
||||
if(pPtr == NULL){
|
||||
snprintf(self->scriptError,510,"ERROR: cannot interpret %s, %s", entry,
|
||||
"require format: motor=value");
|
||||
self->parseOK = 0;
|
||||
return 0;
|
||||
}
|
||||
tuktuk.pDriv = (pIDrivable)FindDrivable(pServ->pSics,tuktuk.name);
|
||||
pCom = FindCommand(pServ->pSics,tuktuk.name);
|
||||
if(!pCom){
|
||||
snprintf(self->scriptError,510,"ERROR: %s not found",tuktuk.name);
|
||||
self->parseOK = 0;
|
||||
return 0;
|
||||
}
|
||||
tuktuk.data = pCom->pData;
|
||||
|
||||
if(tuktuk.pDriv == NULL){
|
||||
snprintf(self->scriptError,510,"ERROR: %s is not drivable",tuktuk.name);
|
||||
self->parseOK = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
pPtr = stptok(pPtr,number,79,"=");
|
||||
tuktuk.value = atof(number);
|
||||
|
||||
LLDnodeAppendFrom(self->motorList,&tuktuk);
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int parseCommandList(pConfigurableVirtualMotor self,
|
||||
char *commandList){
|
||||
char pEntry[256], *pPtr;
|
||||
int status;
|
||||
|
||||
LLDdelete(self->motorList);
|
||||
self->motorList = LLDcreate(sizeof(RealMotor));
|
||||
|
||||
pPtr = commandList;
|
||||
while((pPtr = stptok(pPtr,pEntry,255,",")) != NULL){
|
||||
status = parseEntry(self,pEntry);
|
||||
if(status != 1){
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int startMotorList(pConfigurableVirtualMotor self, SConnection *pCon){
|
||||
RealMotor tuktuk;
|
||||
int status, iRet;
|
||||
|
||||
iRet = LLDnodePtr2First(self->motorList);
|
||||
while(iRet != 0)
|
||||
{
|
||||
LLDnodeDataTo(self->motorList,&tuktuk);
|
||||
status = tuktuk.pDriv->SetValue(tuktuk.data,pCon,tuktuk.value);
|
||||
if(status != 1){
|
||||
return status;
|
||||
}
|
||||
tuktuk.running = 1;
|
||||
LLDnodeDataFrom(self->motorList,&tuktuk);
|
||||
iRet = LLDnodePtr2Next(self->motorList);
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static long ConfSetValue(void *pData, SConnection *pCon, float newValue){
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
||||
char *commandList = NULL;
|
||||
int status;
|
||||
|
||||
assert(self != NULL);
|
||||
|
||||
commandList = invokeMotorScript(self,newValue);
|
||||
if(commandList == NULL){
|
||||
SCWrite(pCon,self->scriptError,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = parseCommandList(self,commandList);
|
||||
if(status != 1){
|
||||
SCWrite(pCon,self->scriptError,eError);
|
||||
return 0;
|
||||
}
|
||||
self->targetValue = newValue;
|
||||
self->targetReached = 0;
|
||||
|
||||
status = startMotorList(self,pCon);
|
||||
if(status != OKOK){
|
||||
HaltMotors(self);
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int ConfCheckLimits(void *pData, float fVal, char *error, int errLen){
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
||||
char *commandList = NULL;
|
||||
int status, iRet;
|
||||
RealMotor tuktuk;
|
||||
|
||||
assert(self != NULL);
|
||||
|
||||
if(self->targetValue != fVal){
|
||||
commandList = invokeMotorScript(self,fVal);
|
||||
if(commandList == NULL){
|
||||
strncpy(error,self->scriptError,errLen);
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = parseCommandList(self,commandList);
|
||||
if(status != 1){
|
||||
strncpy(error,self->scriptError,errLen);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
iRet = LLDnodePtr2First(self->motorList);
|
||||
while(iRet != 0)
|
||||
{
|
||||
LLDnodeDataTo(self->motorList,&tuktuk);
|
||||
status = tuktuk.pDriv->CheckLimits(tuktuk.data,fVal,error,errLen);
|
||||
if(status != 1){
|
||||
return status;
|
||||
}
|
||||
iRet = LLDnodePtr2Next(self->motorList);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static int ConfCheckStatus(void *pData, SConnection *pCon){
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
||||
RealMotor tuktuk;
|
||||
int iRet, status, result;
|
||||
|
||||
result = HWIdle;
|
||||
iRet = LLDnodePtr2First(self->motorList);
|
||||
while(iRet != 0)
|
||||
{
|
||||
LLDnodeDataTo(self->motorList,&tuktuk);
|
||||
if(tuktuk.running == 1){
|
||||
status = tuktuk.pDriv->CheckStatus(tuktuk.data, pCon);
|
||||
switch(status){
|
||||
case HWIdle:
|
||||
tuktuk.running = 0;
|
||||
LLDnodeDataFrom(self->motorList,&tuktuk);
|
||||
break;
|
||||
case HWBusy:
|
||||
result = HWBusy;
|
||||
break;
|
||||
case HWFault:
|
||||
case HWPosFault:
|
||||
return status;
|
||||
break;
|
||||
default:
|
||||
/*
|
||||
this is a programming error and has to be fixed
|
||||
*/
|
||||
assert(0);
|
||||
}
|
||||
}
|
||||
iRet = LLDnodePtr2Next(self->motorList);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static float invokeReadScript(pConfigurableVirtualMotor self,
|
||||
SConnection *pCon){
|
||||
Tcl_Interp *pTcl;
|
||||
int status;
|
||||
|
||||
pTcl = InterpGetTcl(pServ->pSics);
|
||||
|
||||
status = Tcl_Eval(pTcl, self->readScript);
|
||||
if(status != TCL_OK){
|
||||
snprintf(self->scriptError,510,"ERROR: Tcl subsystem reported %s",
|
||||
Tcl_GetStringResult(pTcl));
|
||||
}
|
||||
return atof(Tcl_GetStringResult(pTcl));
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static void checkMotorValues(pConfigurableVirtualMotor self,
|
||||
SConnection *pCon){
|
||||
int iRet;
|
||||
float value;
|
||||
RealMotor tuktuk;
|
||||
char pBueffel[512];
|
||||
|
||||
iRet = LLDnodePtr2First(self->motorList);
|
||||
while(iRet != 0)
|
||||
{
|
||||
LLDnodeDataTo(self->motorList,&tuktuk);
|
||||
value = tuktuk.pDriv->GetValue(tuktuk.data,pCon);
|
||||
value -= tuktuk.value;
|
||||
if(value < .0){
|
||||
value *= -1;
|
||||
}
|
||||
if(value > .1) {
|
||||
snprintf(pBueffel,510,"WARNING: motor %s of position by %f",
|
||||
tuktuk.name,value);
|
||||
SCWrite(pCon,pBueffel,eWarning);
|
||||
return;
|
||||
}
|
||||
iRet = LLDnodePtr2Next(self->motorList);
|
||||
}
|
||||
self->targetReached = 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static float ConfGetValue(void *pData, SConnection *pCon){
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
||||
float currentValue = -9999.99;
|
||||
|
||||
assert(self != NULL);
|
||||
|
||||
if(self->readScript != NULL){
|
||||
currentValue = invokeReadScript(self,pCon);
|
||||
} else {
|
||||
if(self->targetReached == 1){
|
||||
return self->targetValue;
|
||||
} else {
|
||||
checkMotorValues(self,pCon);
|
||||
if(self->targetReached){
|
||||
currentValue = self->targetValue;
|
||||
}
|
||||
}
|
||||
}
|
||||
return currentValue;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static void *GetConfigurableVirtualMotorInterface(void *pData, int iID){
|
||||
pConfigurableVirtualMotor self = NULL;
|
||||
|
||||
self = (pConfigurableVirtualMotor)pData;
|
||||
assert(self);
|
||||
if(iID == DRIVEID){
|
||||
return self->pDriv;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static void KillConfigurableVirtualMotor(void *data){
|
||||
pConfigurableVirtualMotor self = NULL;
|
||||
|
||||
self = (pConfigurableVirtualMotor)data;
|
||||
if(self != NULL){
|
||||
if(self->pDes != NULL){
|
||||
DeleteDescriptor(self->pDes);
|
||||
self->pDes = NULL;
|
||||
}
|
||||
LLDdelete(self->motorList);
|
||||
if(self->scriptName != NULL){
|
||||
free(self->scriptName);
|
||||
self->scriptName = NULL;
|
||||
}
|
||||
if(self->readScript != NULL){
|
||||
free(self->readScript);
|
||||
self->readScript = NULL;
|
||||
}
|
||||
free(self);
|
||||
self = NULL;
|
||||
}
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
int MakeConfigurableVirtualMotor(SConnection *pCon, SicsInterp *pSics,
|
||||
void *data, int argc, char *argv[]){
|
||||
pConfigurableVirtualMotor pNew = NULL;
|
||||
char pBueffel[512];
|
||||
|
||||
if(argc < 2){
|
||||
SCWrite(pCon,"ERROR: need name to create configurable motor",eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pNew = (pConfigurableVirtualMotor)malloc(sizeof(ConfigurableVirtualMotor));
|
||||
if(pNew == NULL){
|
||||
SCWrite(pCon,"ERROR: no memory to create configurable virtual motor",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
memset(pNew,0,sizeof(ConfigurableVirtualMotor));
|
||||
|
||||
pNew->pDes = CreateDescriptor("ConfigurableVirtualMotor");
|
||||
pNew->pDriv = CreateDrivableInterface();
|
||||
pNew->motorList = LLDcreate(sizeof(RealMotor));
|
||||
if(pNew->pDes == NULL || pNew->pDriv == NULL || pNew->motorList < 0){
|
||||
SCWrite(pCon,"ERROR: no memory to create configurable virtual motor",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
assign functions
|
||||
*/
|
||||
pNew->pDes->GetInterface = GetConfigurableVirtualMotorInterface;
|
||||
pNew->pDriv->Halt = HaltMotors;
|
||||
pNew->pDriv->CheckLimits = ConfCheckLimits;
|
||||
pNew->pDriv->SetValue = ConfSetValue;
|
||||
pNew->pDriv->CheckStatus = ConfCheckStatus;
|
||||
pNew->pDriv->GetValue = ConfGetValue;
|
||||
|
||||
/*
|
||||
install command
|
||||
*/
|
||||
if( AddCommand(pSics,argv[1],ConfigurableVirtualMotorAction,
|
||||
KillConfigurableVirtualMotor,pNew) != 1){
|
||||
snprintf(pBueffel,510,"ERROR: duplicate command %s not created",
|
||||
argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
} else {
|
||||
SCSendOK(pCon);
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
int ConfigurableVirtualMotorAction(SConnection *pCon, SicsInterp *pSics,
|
||||
void *data, int argc, char *argv[]){
|
||||
pConfigurableVirtualMotor self = NULL;
|
||||
char pBueffel[512];
|
||||
float value;
|
||||
|
||||
self = (pConfigurableVirtualMotor)data;
|
||||
assert(self != NULL);
|
||||
|
||||
if(argc > 1){
|
||||
strtolower(argv[1]);
|
||||
if(strcmp(argv[1],"drivescript") == 0){
|
||||
if(argc > 2){
|
||||
/* set case */
|
||||
Arg2Text(argc-2,&argv[2],pBueffel,510);
|
||||
self->scriptName = strdup(pBueffel);
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
} else {
|
||||
/* inquiry */
|
||||
if(self->scriptName == NULL){
|
||||
snprintf(pBueffel,510,"%s.drivescript = UNDEFINED", argv[0]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
} else {
|
||||
snprintf(pBueffel,510,"%s.drivescript = %s", argv[0],
|
||||
self->scriptName);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}else if(strcmp(argv[1],"readscript") == 0){
|
||||
if(argc > 2){
|
||||
/* set case */
|
||||
Arg2Text(argc-2,&argv[2],pBueffel,510);
|
||||
self->readScript = strdup(pBueffel);
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
} else {
|
||||
/* inquiry */
|
||||
if(self->readScript == NULL){
|
||||
snprintf(pBueffel,510,"%s.readscript = UNDEFINED", argv[0]);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
} else {
|
||||
snprintf(pBueffel,510,"%s.readscript = %s", argv[0],
|
||||
self->readScript);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
snprintf(pBueffel,5120,"ERROR: subcommand %s to %s unknown",
|
||||
argv[1],argv[0]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
} else {
|
||||
value = self->pDriv->GetValue(self,pCon);
|
||||
snprintf(pBueffel,510,"%s = %f", argv[0],value);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user