- Adapted indenation to new agreed upon system
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
569
confvirtualmot.c
569
confvirtualmot.c
@ -37,193 +37,206 @@ typedef struct {
|
||||
typedef struct {
|
||||
float fVal;
|
||||
char *pName;
|
||||
} EventInfo;
|
||||
} EventInfo;
|
||||
|
||||
/* Data available when registering interest */
|
||||
typedef struct {
|
||||
char *pName;
|
||||
SConnection *pCon;
|
||||
float lastValue;
|
||||
} RegisteredInfo, *pRegisteredInfo;
|
||||
float lastValue;
|
||||
} RegisteredInfo, *pRegisteredInfo;
|
||||
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int HaltMotors(void *pData){
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
||||
static int HaltMotors(void *pData)
|
||||
{
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
|
||||
RealMotor tuktuk;
|
||||
int iRet;
|
||||
|
||||
iRet = LLDnodePtr2First(self->motorList);
|
||||
while(iRet != 0)
|
||||
{
|
||||
LLDnodeDataTo(self->motorList,&tuktuk);
|
||||
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){
|
||||
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");
|
||||
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);
|
||||
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));
|
||||
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);
|
||||
return (char *) Tcl_GetStringResult(pTcl);
|
||||
}
|
||||
|
||||
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int parseEntry(pConfigurableVirtualMotor self,
|
||||
char *entry){
|
||||
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");
|
||||
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);
|
||||
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);
|
||||
|
||||
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,"=");
|
||||
pPtr = stptok(pPtr, number, 79, "=");
|
||||
tuktuk.value = atof(number);
|
||||
|
||||
LLDnodeAppendFrom(self->motorList,&tuktuk);
|
||||
LLDnodeAppendFrom(self->motorList, &tuktuk);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int parseCommandList(pConfigurableVirtualMotor self,
|
||||
char *commandList){
|
||||
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){
|
||||
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){
|
||||
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){
|
||||
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);
|
||||
LLDnodeDataFrom(self->motorList, &tuktuk);
|
||||
iRet = LLDnodePtr2Next(self->motorList);
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
/*------------------------------------------------------------------------*/
|
||||
static long ConfSetValue(void *pData, SConnection *pCon, float newValue){
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
||||
static long ConfSetValue(void *pData, SConnection * pCon, float newValue)
|
||||
{
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
|
||||
char *commandList = NULL;
|
||||
int status;
|
||||
int status;
|
||||
|
||||
assert(self != NULL);
|
||||
|
||||
commandList = invokeMotorScript(self,newValue);
|
||||
if(commandList == NULL){
|
||||
SCWrite(pCon,self->scriptError,eError);
|
||||
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);
|
||||
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){
|
||||
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;
|
||||
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);
|
||||
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);
|
||||
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){
|
||||
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);
|
||||
@ -231,189 +244,196 @@ 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);
|
||||
}
|
||||
if(self->pCon)
|
||||
{
|
||||
SCDeleteConnection(self->pCon);
|
||||
}
|
||||
free(self);
|
||||
}
|
||||
static void KillInfo(void *pData)
|
||||
{
|
||||
pRegisteredInfo self = NULL;
|
||||
|
||||
assert(pData);
|
||||
self = (pRegisteredInfo) pData;
|
||||
if (self->pName) {
|
||||
free(self->pName);
|
||||
}
|
||||
if (self->pCon) {
|
||||
SCDeleteConnection(self->pCon);
|
||||
}
|
||||
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)
|
||||
{
|
||||
pRegisteredInfo pInfo = NULL;
|
||||
char pBueffel[80];
|
||||
EventInfo *pEventData;
|
||||
|
||||
assert(pEvent);
|
||||
assert(pUser);
|
||||
static int InterestCallback(int iEvent, void *pEvent, void *pUser)
|
||||
{
|
||||
pRegisteredInfo pInfo = NULL;
|
||||
char pBueffel[80];
|
||||
EventInfo *pEventData;
|
||||
|
||||
if(!SCisConnected(pInfo->pCon)){
|
||||
return -1;
|
||||
}
|
||||
|
||||
pEventData = (EventInfo *)pEvent;
|
||||
pInfo = (RegisteredInfo *)pUser;
|
||||
assert(pEvent);
|
||||
assert(pUser);
|
||||
|
||||
if (!SCisConnected(pInfo->pCon)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
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);
|
||||
SCWrite(pInfo->pCon, pBueffel, eEvent);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (pInfo->lastValue != pEventData->fVal) {
|
||||
pInfo->lastValue = pEventData->fVal;
|
||||
(pInfo->pCon)->conEventType=POSITION;
|
||||
sprintf(pBueffel,"%s.position = %f ", pInfo->pName, pInfo->lastValue);
|
||||
SCWrite(pInfo->pCon,pBueffel,eEvent);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static int ConfCheckStatus(void *pData, SConnection *pCon){
|
||||
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
|
||||
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){
|
||||
while (iRet != 0) {
|
||||
LLDnodeDataTo(self->motorList, &tuktuk);
|
||||
if (tuktuk.running == 1) {
|
||||
status = tuktuk.pDriv->CheckStatus(tuktuk.data, pCon);
|
||||
switch(status){
|
||||
switch (status) {
|
||||
case HWIdle:
|
||||
tuktuk.running = 0;
|
||||
LLDnodeDataFrom(self->motorList,&tuktuk);
|
||||
break;
|
||||
tuktuk.running = 0;
|
||||
LLDnodeDataFrom(self->motorList, &tuktuk);
|
||||
break;
|
||||
case HWBusy:
|
||||
result = HWBusy;
|
||||
break;
|
||||
result = HWBusy;
|
||||
break;
|
||||
case HWFault:
|
||||
case HWPosFault:
|
||||
return status;
|
||||
break;
|
||||
return status;
|
||||
break;
|
||||
default:
|
||||
/*
|
||||
this is a programming error and has to be fixed
|
||||
*/
|
||||
assert(0);
|
||||
/*
|
||||
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);
|
||||
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)*/)
|
||||
{
|
||||
if (self->posCount >= 10 /*ObVal(self->ParArray,MOVECOUNT) */ ) {
|
||||
event.pName = self->name;
|
||||
event.fVal = self->pDriv->GetValue(self,pCon);
|
||||
event.fVal = self->pDriv->GetValue(self, pCon);
|
||||
InvokeCallBack(self->pCall, MOTDRIVE, &event);
|
||||
self->posCount = 0;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------*/
|
||||
static float invokeReadScript(pConfigurableVirtualMotor self,
|
||||
SConnection *pCon){
|
||||
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));
|
||||
SCWrite(pCon,self->scriptError,eError);
|
||||
if (status != TCL_OK) {
|
||||
snprintf(self->scriptError, 510, "ERROR: Tcl subsystem reported %s",
|
||||
Tcl_GetStringResult(pTcl));
|
||||
SCWrite(pCon, self->scriptError, eError);
|
||||
}
|
||||
return atof(Tcl_GetStringResult(pTcl));
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------*/
|
||||
static void checkMotorValues(pConfigurableVirtualMotor self,
|
||||
SConnection *pCon){
|
||||
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);
|
||||
while (iRet != 0) {
|
||||
LLDnodeDataTo(self->motorList, &tuktuk);
|
||||
value = tuktuk.pDriv->GetValue(tuktuk.data, pCon);
|
||||
value -= tuktuk.value;
|
||||
if(value < .0){
|
||||
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);
|
||||
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;
|
||||
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);
|
||||
if (self->readScript != NULL) {
|
||||
currentValue = invokeReadScript(self, pCon);
|
||||
} else {
|
||||
if(self->targetReached == 1){
|
||||
if (self->targetReached == 1) {
|
||||
return self->targetValue;
|
||||
} else {
|
||||
checkMotorValues(self,pCon);
|
||||
if(self->targetReached){
|
||||
currentValue = self->targetValue;
|
||||
checkMotorValues(self, pCon);
|
||||
if (self->targetReached) {
|
||||
currentValue = self->targetValue;
|
||||
}
|
||||
}
|
||||
}
|
||||
return currentValue;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static void *GetConfigurableVirtualMotorInterface(void *pData, int iID){
|
||||
static void *GetConfigurableVirtualMotorInterface(void *pData, int iID)
|
||||
{
|
||||
pConfigurableVirtualMotor self = NULL;
|
||||
|
||||
self = (pConfigurableVirtualMotor)pData;
|
||||
self = (pConfigurableVirtualMotor) pData;
|
||||
assert(self);
|
||||
if(iID == DRIVEID){
|
||||
if (iID == DRIVEID) {
|
||||
return self->pDriv;
|
||||
} else if(iID == CALLBACKINTERFACE)
|
||||
{
|
||||
} else if (iID == CALLBACKINTERFACE) {
|
||||
return self->pCall;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static void KillConfigurableVirtualMotor(void *data){
|
||||
static void KillConfigurableVirtualMotor(void *data)
|
||||
{
|
||||
pConfigurableVirtualMotor self = NULL;
|
||||
|
||||
self = (pConfigurableVirtualMotor)data;
|
||||
if(self != NULL){
|
||||
if(self->pDes != NULL){
|
||||
|
||||
self = (pConfigurableVirtualMotor) data;
|
||||
if (self != NULL) {
|
||||
if (self->pDes != NULL) {
|
||||
DeleteDescriptor(self->pDes);
|
||||
self->pDes = NULL;
|
||||
}
|
||||
@ -422,11 +442,11 @@ static void KillConfigurableVirtualMotor(void *data){
|
||||
free(self->name);
|
||||
self->name = NULL;
|
||||
}
|
||||
if(self->scriptName != NULL){
|
||||
if (self->scriptName != NULL) {
|
||||
free(self->scriptName);
|
||||
self->scriptName = NULL;
|
||||
}
|
||||
if(self->readScript != NULL){
|
||||
if (self->readScript != NULL) {
|
||||
free(self->readScript);
|
||||
self->readScript = NULL;
|
||||
}
|
||||
@ -434,39 +454,42 @@ static void KillConfigurableVirtualMotor(void *data){
|
||||
self = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
/*------------------------------------------------------------------------*/
|
||||
int MakeConfigurableVirtualMotor(SConnection *pCon, SicsInterp *pSics,
|
||||
void *data, int argc, char *argv[]){
|
||||
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);
|
||||
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);
|
||||
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));
|
||||
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);
|
||||
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
|
||||
*/
|
||||
assign functions
|
||||
*/
|
||||
pNew->pDes->GetInterface = GetConfigurableVirtualMotorInterface;
|
||||
pNew->pDriv->Halt = HaltMotors;
|
||||
pNew->pDriv->CheckLimits = ConfCheckLimits;
|
||||
@ -474,21 +497,20 @@ 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;
|
||||
}
|
||||
/* 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);
|
||||
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);
|
||||
@ -496,9 +518,11 @@ int MakeConfigurableVirtualMotor(SConnection *pCon, SicsInterp *pSics,
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
int ConfigurableVirtualMotorAction(SConnection *pCon, SicsInterp *pSics,
|
||||
void *data, int argc, char *argv[]){
|
||||
int ConfigurableVirtualMotorAction(SConnection * pCon, SicsInterp * pSics,
|
||||
void *data, int argc, char *argv[])
|
||||
{
|
||||
pConfigurableVirtualMotor self = NULL;
|
||||
char pBueffel[512];
|
||||
float value;
|
||||
@ -506,84 +530,83 @@ int ConfigurableVirtualMotorAction(SConnection *pCon, SicsInterp *pSics,
|
||||
long lID;
|
||||
pRegisteredInfo pRegInfo = NULL;
|
||||
|
||||
self = (pConfigurableVirtualMotor)data;
|
||||
self = (pConfigurableVirtualMotor) data;
|
||||
assert(self != NULL);
|
||||
|
||||
if(argc > 1){
|
||||
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 = SCCopyConnection(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, MOTDRIVE,
|
||||
InterestCallback, pRegInfo, KillInfo);
|
||||
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 {
|
||||
snprintf(pBueffel,5120,"ERROR: subcommand %s to %s unknown",
|
||||
argv[1],argv[0]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
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 = SCCopyConnection(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, MOTDRIVE,
|
||||
InterestCallback, pRegInfo, KillInfo);
|
||||
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);
|
||||
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