581 lines
16 KiB
C
581 lines
16 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
|
|
interest added: Ferdi Franceschini, April 2006
|
|
-------------------------------------------------------------------------*/
|
|
#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;
|
|
|
|
/* Data passed by event generating object */
|
|
typedef struct {
|
|
float fVal;
|
|
char *pName;
|
|
} EventInfo;
|
|
|
|
/* Data available when registering interest */
|
|
typedef struct {
|
|
char *pName;
|
|
SConnection *pCon;
|
|
float lastValue;
|
|
} RegisteredInfo, *pRegisteredInfo;
|
|
|
|
/*---------------------------------------------------------------------*/
|
|
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;
|
|
self->posCount = 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,tuktuk.value,error,errLen);
|
|
if(status != 1){
|
|
return status;
|
|
}
|
|
iRet = LLDnodePtr2Next(self->motorList);
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
static void KillInfo(void *pData)
|
|
{
|
|
pRegisteredInfo self = NULL;
|
|
|
|
assert(pData);
|
|
self = (pRegisteredInfo)pData;
|
|
if(self->pName)
|
|
{
|
|
free(self->pName);
|
|
}
|
|
free(self);
|
|
}
|
|
/*------------------- The CallBack function for interest ------------------
|
|
* iEvent: Event ID, see event.h for SICS events
|
|
* pEvent: May contain data from event generating object
|
|
* pUser: Data available when registering interest, see RegisteredInfo struct
|
|
* defined above for available info */
|
|
static int InterestCallback(int iEvent, void *pEvent, void *pUser,
|
|
commandContext cc)
|
|
{
|
|
pRegisteredInfo pInfo = NULL;
|
|
char pBueffel[80];
|
|
EventInfo *pEventData;
|
|
|
|
assert(pEvent);
|
|
assert(pUser);
|
|
|
|
pEventData = (EventInfo *)pEvent;
|
|
pInfo = (RegisteredInfo *)pUser;
|
|
|
|
if (pInfo->lastValue != pEventData->fVal) {
|
|
pInfo->lastValue = pEventData->fVal;
|
|
(pInfo->pCon)->conEventType=POSITION;
|
|
sprintf(pBueffel,"%s.position = %f ", pInfo->pName, pInfo->lastValue);
|
|
SCWriteInContext(pInfo->pCon,pBueffel,eEvent,cc);
|
|
}
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int ConfCheckStatus(void *pData, SConnection *pCon){
|
|
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
|
RealMotor tuktuk;
|
|
int iRet, status, result;
|
|
EventInfo event;
|
|
|
|
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);
|
|
}
|
|
|
|
if (result == HWIdle) {
|
|
event.pName = self->name;
|
|
event.fVal = self->pDriv->GetValue(self,pCon);
|
|
InvokeCallBack(self->pCall, MOTDRIVE, &event);
|
|
} else if (result == HWBusy) {
|
|
self->posCount++;
|
|
if(self->posCount >= 10/*ObVal(self->ParArray,MOVECOUNT)*/)
|
|
{
|
|
event.pName = self->name;
|
|
event.fVal = self->pDriv->GetValue(self,pCon);
|
|
InvokeCallBack(self->pCall, MOTDRIVE, &event);
|
|
self->posCount = 0;
|
|
}
|
|
}
|
|
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;
|
|
} else if(iID == CALLBACKINTERFACE)
|
|
{
|
|
return self->pCall;
|
|
}
|
|
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->name != NULL) {
|
|
free(self->name);
|
|
self->name = NULL;
|
|
}
|
|
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->name = strdup(argv[1]);
|
|
pNew->posCount = 0;
|
|
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;
|
|
|
|
/* initialise callback interface */
|
|
pNew->pCall = CreateCallBackInterface();
|
|
if(!pNew->pCall)
|
|
{
|
|
KillConfigurableVirtualMotor(pNew);
|
|
return 0;
|
|
}
|
|
/*
|
|
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;
|
|
int iRet;
|
|
long lID;
|
|
pRegisteredInfo pRegInfo = NULL;
|
|
|
|
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 if(strcmp(argv[1],"interest") == 0)
|
|
{
|
|
pRegInfo = (pRegisteredInfo)malloc(sizeof(RegisteredInfo));
|
|
if(!pRegInfo)
|
|
{
|
|
SCWrite(pCon,"ERROR: out of memory in confvirtualmot.c",eError);
|
|
return 0;
|
|
}
|
|
pRegInfo->pName = strdup(argv[0]);
|
|
pRegInfo->pCon = pCon;
|
|
value = ConfGetValue(self,pCon);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pBueffel,"Failed to register interest, Reason:Error obtaining current position for %s",argv[0]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
pRegInfo->lastValue = value;
|
|
|
|
lID = RegisterCallback(self->pCall, SCGetContext(pCon),MOTDRIVE, InterestCallback, pRegInfo, KillInfo);
|
|
SCRegister(pCon,pSics, self->pCall,lID);
|
|
SCSendOK(pCon);
|
|
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;
|
|
}
|
|
}
|