468 lines
13 KiB
C
468 lines
13 KiB
C
/*-----------------------------------------------------------------------
|
|
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 (char *)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;
|
|
}
|
|
}
|