sics/sctdriveobj.c

/**
 * 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.
 * 
 * Some cooperation from the node is required: It has to provide
 * certain properties the value of which define scripts which 
 * have to be called at various stages. These are:
 * 
 *    checklimits, for limits checking
 *    checkstatus, for evaluating progress
 *    halt       , for halting things
 *
 * If no checklimits script is given, nothing is checked.
 * If no checkstatus scripts is given, the value of the status property is
 * returned. In that case the status value should be updated by some poll script.
 * If no halt script is given, the status is set to idle. M.Z. Sept. 08
 *
 * 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;
  SConnection *pCon;
} DrivObjPriv, *pDrivObjPriv;
/*--------------------------------------------------------------*/
static void KillDrivePriv(void *data)
{
  pDrivObjPriv priv = (pDrivObjPriv) data;
  if (priv == NULL) {
    return;
  }
  if (priv->pDriv != NULL) {
    free(priv->pDriv);
  }
  if (priv->pCon != NULL) {
    SCDeleteConnection(priv->pCon);
  }
  free(priv);
}

/*---------------------------------------------------------------*/
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", 1,
                 pPriv->pCon);
  } 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, "%g", val);
  /* set target. This not yet done, as SetValue is called later */
  SetHdbProperty(self->objectNode, "target", script);
  if (GetHdbProperty(self->objectNode, "checklimits", script, 1024)) {
    status = SctCallInContext(pServ->dummyCon, script,
                              self->objectNode, pPriv->c, &result);
    if (SctDebugConn(pPriv->c)) {
      SCPf(SCPureSockWrite, SctDebugConn(pPriv->c), eWarning,
           "script %s called with result %s\n ", script, result);
    }
    if (status == 0) {
      strlcpy(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;
  if (pPriv->pCon != NULL) {
    SCDeleteConnection(pPriv->pCon);
  }
  pPriv->pCon = SCCopyConnection(pCon);

  v = MakeHdbFloat(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 duely 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;
  int ret;
  
  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) {
      ret = HWBusy;
      goto Return;
    }
  }

  /*
   * 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);
      ret = HWFault;
      result = "error";
      goto Return;
    }
    result = script;
  } else {
    status = SctCallInContext(pCon, script, self->objectNode,
                              pPriv->c, &result);
    if (status == 0) {
      SCPrintf(pCon, eError, " script %s returned %s", script, result);
      ret = HWFault;
      result = "error";
      goto Return;
    }
    if (SctDebugConn(pPriv->c)) {
      SCPf(SCPureSockWrite, SctDebugConn(pPriv->c), eError,
           " script %s returned %s", script, result);
    }
  }
  if (strstr(result, "run") != NULL) {
    ret = HWBusy;
  } else if (strstr(result, "posfault") != NULL) {
    ret = HWPosFault;
  } else if (strstr(result, "error") != NULL) {
    ret = HWFault;
  } else if (strstr(result, "idle") != NULL) {
    ret = HWIdle;
  } else {
    SCPrintf(pCon, eError,
             "ERROR: invalid status code %s returned from checkstatus script",
             result);
    ret = HWFault;
  }
Return:
  if (GetHdbProperty(self->objectNode, "updatestatus", script, 1024)) {
    status = SctCallInContext(pCon, script, self->objectNode,
                              pPriv->c, &result);
    if (status == 0) {
      SCPrintf(pCon, eError, " %s returned %s", script, result);
    }
  }
  return ret;
}

/*----------------------------------------------------------------
 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 && GetDescriptorKey(self->pDes, "creationCommand") == NULL   /* it's not a dynamic object */
      && self->pDes != NULL) {
    self->objectNode = NULL;    /* do not call RemoveHdbNodeFromParent in KillSICSOBJ */
    self->pDes->parNode = NULL; /* do not kill the node in KillSICSOBJ/DeleteDescriptor */
  }
  KillSICSOBJ(self);
}

/*-----------------------------------------------------------------------------*/
pSICSOBJ MakeSctDriveObj(pHdb node, char *class, SctController * c,
                         int doNotKillNode)
{
  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);
  pNew->pDes->SaveStatus = SaveSICSOBJ;
  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;
  pPriv->doNotKillNode = doNotKillNode;
  pNew->pDes->parNode = pNew->objectNode;
  pNew->pDes->GetInterface = SCTDRIVGetInterface;
  pNew->pPrivate = pPriv;
  pNew->KillPrivate = KillDrivePriv;
  return pNew;
}

/*--------------------------------------------------------------------------
 * This actually has two syntaxes:
 * makesctdrive name path-to-existing-node  class SctController
 * makesctdrive name float 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, doNotKillNode;
  hdbValue val;
  char *sctName;
  char *class;

  if (argc < 5)
    goto Usage;

  if (argc == 5) {
    node = FindHdbNode(NULL, argv[2], pCon);
    if (node == NULL) {
      SCPrintf(pCon, eError, "ERROR: node %s not found", argv[2]);
      goto Usage;
    }
    sctName = argv[4];
    class = argv[3];
    doNotKillNode = 1;          /* the node is owned by someone else */
  } else {
    /* convert datatype */
    type = convertHdbType(argv[2]);
    /* convert privilege */
    priv = decodeSICSPriv(argv[3]);
    if (priv < 0 || type != HIPFLOAT)
      goto Usage;
    node = MakeSICSHdbPar(argv[1], priv, MakeHdbFloat(0.0));
    if (node == NULL) {
      SCWrite(pCon, "ERROR: node creation failed", eError);
      return 0;
    }
    SetHdbProperty(node, "sicscommand", argv[1]);
    sctName = argv[5];
    class = argv[4];
    doNotKillNode = 0;          /* kill the node with the command */
  }

  pSct = FindCommandData(pSics, sctName, "SctController");
  if (pSct == NULL) {
    SCPrintf(pCon, eError, "ERROR: SctController %s not found", sctName);
    goto Usage;
  }
  c = (SctController *) pSct->pPrivate;
  pNew = MakeSctDriveObj(node, class, c, doNotKillNode);
  SetHdbProperty(node, "objectName", argv[1]);

  if (pNew == NULL) {
    SCWrite(pCon, "ERROR: failed to create drive object", eError);
    return 0;
  }
  if (strcasecmp(argv[0], "dynsctdriveobj") == 0) {
    /* make object dynamic by defining a creation command */
    SetDescriptorKey(pNew->pDes, "creationCommand", "0");
    RegisterSICSOBJKillCmd(pNew, argv[1]);
  }
  SetHdbProperty(pNew->objectNode, "sicsdev", argv[1]);
  SetHdbProperty(pNew->objectNode, "type", "drivable");
  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;
Usage:
  SCPrintf(pCon, eError,
           "ERROR: should be  %s <object name> <existing node> <class> <controller>",
           argv[0]);
  SCPrintf(pCon, eError,
           "ERROR:        or  %s <object name> <type> <priv> <class> <controller>",
           argv[0]);
  return 0;
}
/*---------------------------------------------------------------*/
void SctDriveObjInit(void)
{
  AddCmd("makesctdriveobj", SctMakeDriveObject);
  AddCmd("dynsctdriveobj", SctMakeDriveObject);
}