linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added 'interest' method

Browse Source
This commit is contained in:
hauser_n
2006-05-01 04:53:27 +00:00
parent 48fc610da6
commit 228ec3fd9e
1 changed files with 116 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
confvirtualmot.c
View File

@ -8,6 +8,7 @@
COPYRIGHT: see file COPYRIGHT
Mark Koennecke, August 2004
interest added: Ferdi Franceschini, April 2006
-------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
@ -31,6 +32,20 @@ typedef struct {
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;
@ -172,6 +187,7 @@ static long ConfSetValue(void *pData, SConnection *pCon, float newValue){
}
self->targetValue = newValue;
self->targetReached = 0;
self->posCount = 0;
status = startMotorList(self,pCon);
if(status != OKOK){
@ -214,11 +230,51 @@ static int ConfCheckLimits(void *pData, float fVal, char *error, int errLen){
}
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);
@ -248,6 +304,21 @@ static int ConfCheckStatus(void *pData, SConnection *pCon){
}
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;
}
/*---------------------------------------------------------------------*/
@ -320,7 +391,10 @@ static void *GetConfigurableVirtualMotorInterface(void *pData, int iID){
self = (pConfigurableVirtualMotor)pData;
assert(self);
if(iID == DRIVEID){
return self->pDriv;
return self->pDriv;
} else if(iID == CALLBACKINTERFACE)
{
return self->pCall;
}
return NULL;
}
@ -335,6 +409,10 @@ static void KillConfigurableVirtualMotor(void *data){
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;
@ -366,6 +444,8 @@ int MakeConfigurableVirtualMotor(SConnection *pCon, SicsInterp *pSics,
}
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));
@ -385,6 +465,13 @@ int MakeConfigurableVirtualMotor(SConnection *pCon, SicsInterp *pSics,
pNew->pDriv->CheckStatus = ConfCheckStatus;
pNew->pDriv->GetValue = ConfGetValue;
/* initialise callback interface */
pNew->pCall = CreateCallBackInterface();
if(!pNew->pCall)
{
KillConfigurableVirtualMotor(pNew);
return 0;
}
/*
install command
*/
@ -406,6 +493,9 @@ int ConfigurableVirtualMotorAction(SConnection *pCon, SicsInterp *pSics,
pConfigurableVirtualMotor self = NULL;
char pBueffel[512];
float value;
int iRet;
long lID;
pRegisteredInfo pRegInfo = NULL;
self = (pConfigurableVirtualMotor)data;
assert(self != NULL);
@ -452,7 +542,30 @@ int ConfigurableVirtualMotorAction(SConnection *pCon, SicsInterp *pSics,
return 1;
}
}
} else {
} 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);