- More debugging of scriptcontext, mostly working now
- Created driveable SICSobj for standalone and as an adapter
This commit is contained in:
koennecke
337
sctdriveobj.c
Normal file
337
sctdriveobj.c
Normal file
@ -0,0 +1,337 @@
|
||||
/**
|
||||
* This is a base class for any drivable object which uses the new
|
||||
* scriptcontext to control the actual driving operation. This can also
|
||||
* serve as a drivable adapter to nodes in other objects.
|
||||
*
|
||||
* copyright: see file COPYRIGHT
|
||||
*
|
||||
* Mark Koennecke, June-July 2008
|
||||
*/
|
||||
#include <sics.h>
|
||||
#include <tcl.h>
|
||||
#include <sicsobj.h>
|
||||
#include <scriptcontext.h>
|
||||
#include <sicshipadaba.h>
|
||||
|
||||
typedef struct {
|
||||
pIDrivable pDriv;
|
||||
SctController *c;
|
||||
int doNotKillNode;
|
||||
} DrivObjPriv, *pDrivObjPriv;
|
||||
/*---------------------------------------------------------------*/
|
||||
static void *SCTDRIVGetInterface(void *data, int iD){
|
||||
pSICSOBJ self = NULL;
|
||||
pDrivObjPriv pPriv;
|
||||
|
||||
self = (pSICSOBJ)data;
|
||||
pPriv = (pDrivObjPriv)self->pPrivate;
|
||||
if(self != NULL && pPriv != NULL && iD == DRIVEID){
|
||||
if (self->objectNode == NULL) return NULL;
|
||||
return pPriv->pDriv;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return either OKOK or HWFault when thing
|
||||
go wrong. However, the return value of Halt is usually ignored!
|
||||
------------------------------------------------------------------*/
|
||||
static int SCTDRIVHalt(void *data) {
|
||||
pSICSOBJ self = NULL;
|
||||
pDrivObjPriv pPriv;
|
||||
char dummy[16];
|
||||
|
||||
self = (pSICSOBJ)data;
|
||||
pPriv = (pDrivObjPriv)self->pPrivate;
|
||||
if (GetHdbProperty(self->objectNode,"halt", dummy, sizeof dummy)) {
|
||||
SctQueueNode(pPriv->c, self->objectNode, HaltPRIO, "halt", NULL);
|
||||
} else if (GetHdbProperty(self->objectNode, "status", dummy, sizeof dummy)) {
|
||||
SetHdbProperty(self->objectNode, "status", "idle");
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return either 1 or 0. 1 means the position can
|
||||
be reached, 0 NOT
|
||||
If 0, error shall contain up to errlen characters of information
|
||||
about which limit was violated
|
||||
------------------------------------------------------------------*/
|
||||
static int SCTDRIVCheckLimits(void *data, float val,
|
||||
char *error, int errlen){
|
||||
pSICSOBJ self = NULL;
|
||||
pDrivObjPriv pPriv;
|
||||
char script[1024];
|
||||
int status;
|
||||
Tcl_Interp *pTcl = NULL;
|
||||
char *result;
|
||||
|
||||
self = (pSICSOBJ)data;
|
||||
pPriv = (pDrivObjPriv)self->pPrivate;
|
||||
snprintf(script,1024,"%f", val);
|
||||
SetHdbProperty(self->objectNode,"target", script);
|
||||
if(GetHdbProperty(self->objectNode,"checklimits",script,1024)){
|
||||
status = SctCallInContext(pServ->dummyCon, script,
|
||||
self->objectNode, pPriv->c, &result);
|
||||
if(SctVerbose(pPriv->c)){
|
||||
SCPrintf(pServ->dummyCon, eWarning, "script %s called with result %s\n ",
|
||||
script, result);
|
||||
}
|
||||
if(status == 0){
|
||||
strncpy(error,result,errlen);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return 0 when a limit problem occurred
|
||||
OKOK when the motor was successfully started
|
||||
HWFault when a problem occured starting the device
|
||||
Possible errors shall be printed to pCon
|
||||
For real motors, this is supposed to try at least three times
|
||||
to start the motor in question
|
||||
val is the value to drive the motor too
|
||||
------------------------------------------------------------------*/
|
||||
static long SCTDRIVSetValue(void *data, SConnection *pCon, float val){
|
||||
pSICSOBJ self = NULL;
|
||||
pDrivObjPriv pPriv;
|
||||
int status;
|
||||
hdbValue v;
|
||||
|
||||
self = (pSICSOBJ)data;
|
||||
pPriv = (pDrivObjPriv)self->pPrivate;
|
||||
|
||||
v.dataType = HIPFLOAT;
|
||||
v.v.doubleValue = (double)val;
|
||||
SetHdbProperty(self->objectNode,"writestatus", "start");
|
||||
status = SetHipadabaPar(self->objectNode, v, pCon);
|
||||
if(status == 1){
|
||||
return OKOK;
|
||||
} else {
|
||||
return HWFault;
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------
|
||||
Checks the status of a running motor. Possible return values
|
||||
HWBusy The motor is still running
|
||||
OKOK or HWIdle when the motor finished driving
|
||||
HWFault when a hardware problem ocurred
|
||||
HWPosFault when the hardware cannot reach a position
|
||||
Errors are duly to be printed to pCon
|
||||
For real motors CheckStatus again shall try hard to fix any
|
||||
issues with the motor
|
||||
------------------------------------------------------------------*/
|
||||
static int SCTDRIVCheckStatus(void *data, SConnection *pCon){
|
||||
pSICSOBJ self = NULL;
|
||||
pDrivObjPriv pPriv;
|
||||
char script[1024];
|
||||
int status;
|
||||
Tcl_Interp *pTcl = NULL;
|
||||
char *result;
|
||||
SConnection *con;
|
||||
|
||||
self = (pSICSOBJ)data;
|
||||
pPriv = (pDrivObjPriv)self->pPrivate;
|
||||
|
||||
/*
|
||||
* check if the write command has gone through
|
||||
*/
|
||||
if(GetHdbProperty(self->objectNode,"writestatus", script,1024)){
|
||||
if(strcmp(script,"start") == 0){
|
||||
return HWBusy;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* run the checkstatus script
|
||||
*/
|
||||
if(!GetHdbProperty(self->objectNode,"checkstatus",script,1024)){
|
||||
if (!GetHdbProperty(self->objectNode,"status",script,1024)){
|
||||
SCWrite(pCon,
|
||||
"ERROR: configuration problem: no checkstatus script!", eError);
|
||||
return HWFault;
|
||||
}
|
||||
result = script;
|
||||
} else {
|
||||
status = SctCallInContext(pCon,script, self->objectNode,
|
||||
pPriv->c, &result);
|
||||
if (status == 0) {
|
||||
SCPrintf(pCon,eError," script %s returned %s",
|
||||
script, result);
|
||||
return HWFault;
|
||||
}
|
||||
if(SctVerbose(pPriv->c)){
|
||||
SCPrintf(pCon,eError," script %s returned %s",
|
||||
script, result);
|
||||
}
|
||||
}
|
||||
if(strstr(result,"busy") != NULL){
|
||||
return HWBusy;
|
||||
} else if(strstr(result,"posfault") != NULL){
|
||||
return HWPosFault;
|
||||
} else if(strstr(result,"fault") != NULL){
|
||||
return HWFault;
|
||||
} else if(strstr(result,"idle") != NULL){
|
||||
return HWIdle;
|
||||
} else {
|
||||
SCPrintf(pCon,eError,
|
||||
"ERROR: invalid status code %s returned from checkstatus script",
|
||||
result);
|
||||
return HWFault;
|
||||
}
|
||||
return HWFault;
|
||||
}
|
||||
/*----------------------------------------------------------------
|
||||
GetValue is supposed to read a motor position
|
||||
On errors, -99999999.99 is returned and messages printed to pCon
|
||||
------------------------------------------------------------------*/
|
||||
static float SCTDRIVGetValue(void *data, SConnection *pCon){
|
||||
pSICSOBJ self = NULL;
|
||||
pDrivObjPriv pPriv;
|
||||
float val = -99999999.99;
|
||||
int status;
|
||||
char error[256];
|
||||
hdbValue v;
|
||||
|
||||
self = (pSICSOBJ)data;
|
||||
pPriv = (pDrivObjPriv)self->pPrivate;
|
||||
if(GetHdbProperty(self->objectNode,"geterror", error, 256)){
|
||||
SCWrite(pCon,error, eError);
|
||||
return val;
|
||||
}
|
||||
return (float)self->objectNode->value.v.doubleValue;
|
||||
}
|
||||
/*-----------------------------------------------------------------*/
|
||||
void AssignSctDrive(pIDrivable pDriv){
|
||||
pDriv->Halt = SCTDRIVHalt;
|
||||
pDriv->CheckLimits = SCTDRIVCheckLimits;
|
||||
pDriv->SetValue = SCTDRIVSetValue;
|
||||
pDriv->CheckStatus = SCTDRIVCheckStatus;
|
||||
pDriv->GetValue = SCTDRIVGetValue;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static void KillDriveOBJ(void *data){
|
||||
pSICSOBJ self = (pSICSOBJ)data;
|
||||
pDrivObjPriv pPriv;
|
||||
|
||||
if(self == NULL){
|
||||
return;
|
||||
}
|
||||
|
||||
pPriv = (pDrivObjPriv)self->pPrivate;
|
||||
if(pPriv->doNotKillNode && self->pDes != NULL){
|
||||
if(self->pDes->name) free(self->pDes->name);
|
||||
if(self->pDes->pKeys) IFDeleteOptions(self->pDes->pKeys);
|
||||
free(self->pDes);
|
||||
} else {
|
||||
DeleteDescriptor(self->pDes); /* kill descriptor including node */
|
||||
}
|
||||
if(self->KillPrivate != NULL && self->pPrivate != NULL){
|
||||
self->KillPrivate(self->pPrivate);
|
||||
}
|
||||
RemoveHdbNodeFromParent(self->objectNode, pServ->dummyCon);
|
||||
free(self);
|
||||
}
|
||||
/*-----------------------------------------------------------------------------*/
|
||||
pSICSOBJ MakeSctDriveObj(pHdb node, char *class, SctController *c){
|
||||
pSICSOBJ pNew = NULL;
|
||||
pDrivObjPriv pPriv = NULL;
|
||||
hdbValue val;
|
||||
|
||||
pNew = (pSICSOBJ)malloc(sizeof(SICSOBJ));
|
||||
pPriv = (pDrivObjPriv)malloc(sizeof(DrivObjPriv));
|
||||
if(pNew == NULL || pPriv == NULL){
|
||||
return NULL;
|
||||
}
|
||||
memset(pNew,0,sizeof(SICSOBJ));
|
||||
memset(pPriv,0,sizeof(DrivObjPriv));
|
||||
|
||||
pNew->pDes = CreateDescriptor(class);
|
||||
pPriv->pDriv = CreateDrivableInterface();
|
||||
if(pNew->pDes == NULL || pPriv->pDriv == NULL){
|
||||
free(pNew);
|
||||
free(pPriv);
|
||||
return(NULL);
|
||||
}
|
||||
pNew->objectNode = node;
|
||||
AssignSctDrive(pPriv->pDriv);
|
||||
pPriv->c = c;
|
||||
pNew->pDes->parNode = pNew->objectNode;
|
||||
pNew->pDes->GetInterface = SCTDRIVGetInterface;
|
||||
pNew->pPrivate = pPriv;
|
||||
pNew->KillPrivate = DefaultKill;
|
||||
return pNew;
|
||||
}
|
||||
/*--------------------------------------------------------------------------
|
||||
* This actually has two syntaxes:
|
||||
* makesctdrive name path-to-existing-node class SctController
|
||||
* makesctdrive name type priv class SctController
|
||||
*--------------------------------------------------------------------------*/
|
||||
int SctMakeDriveObject(SConnection *pCon, SicsInterp *pSics, void *object,
|
||||
int argc, char *argv[]) {
|
||||
pHdb node = NULL;
|
||||
pDrivObjPriv pPriv = NULL;
|
||||
pSICSOBJ pNew = NULL;
|
||||
pSICSOBJ pSct = NULL;
|
||||
SctController *c = NULL;
|
||||
int priv, type, status;
|
||||
hdbValue val;
|
||||
|
||||
if(argc < 5){
|
||||
SCWrite(pCon,"ERROR: not enough arguments to SctMakeDriveObject", eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
node = FindHdbNode(NULL,argv[2], pCon);
|
||||
if(node != NULL){
|
||||
pSct = FindCommandData(pSics,argv[4], "SctController");
|
||||
if(pSct == NULL){
|
||||
SCPrintf(pCon,eError, "ERROR: SctController %s not found", argv[4]);
|
||||
return 0;
|
||||
}
|
||||
c = (SctController *)pSct->pPrivate;
|
||||
pNew = MakeSctDriveObj(node,argv[3],c);
|
||||
pPriv = (pDrivObjPriv)pNew->pPrivate;
|
||||
pPriv->doNotKillNode = 1; /* the node is owned by someone else*/
|
||||
} else {
|
||||
/* convert privilege */
|
||||
priv = decodeSICSPriv(argv[3]);
|
||||
/* convert datatype */
|
||||
strtolower(argv[4]);
|
||||
type = convertHdbType(argv[2]);
|
||||
if (type == HIPNONE) {
|
||||
node = MakeHipadabaNode(argv[1], HIPNONE, 1);
|
||||
} else {
|
||||
val = makeHdbValue(type,0);
|
||||
node = MakeSICSHdbPar(argv[1], priv, val);
|
||||
ReleaseHdbValue(&val);
|
||||
}
|
||||
if(node == NULL){
|
||||
SCWrite(pCon,"ERROR: node creation failed", eError);
|
||||
return 0;
|
||||
}
|
||||
c = FindCommandData(pSics,argv[5], "SctController");
|
||||
if(c == NULL){
|
||||
SCPrintf(pCon,eError, "ERROR: SctController %s not found", argv[4]);
|
||||
return 0;
|
||||
}
|
||||
pNew = MakeSctDriveObj(node,argv[3],c);
|
||||
}
|
||||
if(pNew == NULL){
|
||||
SCWrite(pCon,"ERROR: failed to create drive object", eError);
|
||||
return 0;
|
||||
}
|
||||
status = AddCommand(pSics,
|
||||
argv[1],
|
||||
InterInvokeSICSOBJ,
|
||||
KillDriveOBJ,
|
||||
pNew);
|
||||
if(status != 1){
|
||||
KillSICSOBJ(pNew);
|
||||
SCPrintf(pCon,eError,"ERROR: failed create duplicate command %s", argv[1]);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user