sics/simdriv.c

/*-----------------------------------------------------------------------------

  SINQ suffers from a severe lack of hardware. In order to do something
  inspite of this, there is the Simulated motor for testing software and
  perhaps later on as a simulation engine for testing batch-files.
	
  Mark Koennecke,    November 1996

  Prepared for simulation mode by providing for zero failure operation
  Mark Koennecke, October 2001

  Allowed for changing hardware limits: Mark Koennecke, April 2003

  Copyright:

  Labor fuer Neutronenstreuung
  Paul Scherrer Institut
  CH-5423 Villigen-PSI


  The authors hereby grant permission to use, copy, modify, distribute,
  and license this software and its documentation for any purpose, provided
  that existing copyright notices are retained in all copies and that this
  notice is included verbatim in any distributions. No written agreement,
  license, or royalty fee is required for any of the authorized uses.
  Modifications to this software may be copyrighted by their authors
  and need not follow the licensing terms described here, provided that
  the new terms are clearly indicated on the first page of each file where
  they apply.

  IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
  FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
  ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
  DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.

  THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE
  IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
  NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
  MODIFICATIONS.
----------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <time.h>
#include <strlutil.h>

#include "fortify.h"
#include "conman.h"
#include "modriv.h"
#include "splitter.h"
#include "logv2.h"
/*-------------------------------------------------------------------------*/
static float SimRandom(void)
{
  float fVal;

  fVal = ((float) rand() / (float) RAND_MAX) * 100.0;
  return fVal;
}

/*---------------------------------------------------------------------------*/
static int RunComplete(SIMDriv * self)
{
  int sign;
  time_t now;

  if (self->iTime == 0) {
    return 1;
  }
  now = time(NULL);
  /* move */
  if (self->fTarget > self->fPos) {
    sign = 1;
  } else {
    sign = -1;
  }
  self->fPos += sign * self->fSpeed * (now - self->iTime);
  self->iTime = now;
  if (sign * (self->fPos - self->fTarget) > 0) {
    self->fPos = self->fTarget;
    return 1;
  }
  return 0;
}

/*----------------------------------------------------------------------------*/
static int GetSimPos(void *self, float *fPos)
{
  SIMDriv *pDriv;

  assert(self);
  pDriv = (SIMDriv *) self;

  /*
     no error checking case
   */
  if (pDriv->fFailure < .0) {
    *fPos = pDriv->fPos;
    return OKOK;
  }

  if (SimRandom() < pDriv->fFailure) {
    *fPos = SimRandom();
    return HWFault;
  }

  *fPos = pDriv->fPos;
  return OKOK;
}

/*----------------------------------------------------------------------------*/
static int SimRun(void *self, float fVal)
{
  SIMDriv *pDriv;

  assert(self);
  pDriv = (SIMDriv *) self;

  /*
     no failure, no wait situation
   */
  if (pDriv->fFailure < .0) {
    pDriv->fPos = fVal;
    return OKOK;
  }


  /* set start time  */
  pDriv->fTarget = fVal;
  pDriv->iTime = time(NULL);

  /* in a fifth the failures, simply die, else simply do not find pos */
  if (SimRandom() < (pDriv->fFailure / 5)) {
    return HWFault;
  } else if (SimRandom() < pDriv->fFailure) {
    pDriv->fPos = fVal - 1.;
    return OKOK;
  } else {
    /* pDriv->fPos = fVal; */
    return OKOK;
  }
}

/*--------------------------------------------------------------------------*/
static void SimError(void *self, int *iCode, char *error, int iErrLen)
{
  assert(self);

  if (RunComplete((SIMDriv *) self)) {
    *iCode = 56;
    strlcpy(error, "ERROR: HW: HahahahahahahHahahHahaha-Mmmpfff", iErrLen);
  } else {
    *iCode = 12;
    strlcpy(error, "Motor still creeping along", iErrLen - 1);
  }
}

/*---------------------------------------------------------------------------*/
static int SimFix(void *self, int iError, float fNew)
{
  SIMDriv *pDriv;
  float fRand;

  /* return the three values MOTREDO, MOTFAIL, MOTOK with a third
     randomness
   */
  assert(self);
  fRand = SimRandom();
  pDriv = (SIMDriv *) self;

  if (iError == 12) {
    return MOTREDO;
  }

  Log(ERROR,"dev","%s","Simulated Motor dying randomly");
  if (fRand < 0.3333) {
    return MOTOK;
  } else if (fRand < 0.66666) {
    return MOTREDO;
  } else {
    pDriv->iTime = 0;
    return MOTFAIL;
  }
}

/*--------------------------------------------------------------------------*/
static int SimHalt(void *self)
{
  SIMDriv *pDriv;
  assert(self);

  pDriv = (SIMDriv *) self;
  pDriv->iTime = 0;
  return OKOK;
}

/*--------------------------------------------------------------------------*/
static int SimStat(void *self)
{
  SIMDriv *pDriv;

  assert(self);
  pDriv = (SIMDriv *) self;

  /*
     no wait, no fail situation
   */
  if (pDriv->fFailure < .0) {
    return HWIdle;
  }

  if (RunComplete(pDriv)) {
    return HWIdle;
  } else {
    if (SimRandom() < pDriv->fFailure / 2) {
      return HWPosFault;
    } else if (SimRandom() < pDriv->fFailure) {
      return HWFault;
    }
    return HWBusy;
  }
}

/*-----------------------------------------------------------------------*/
static int SimSetPar(void *self, SConnection * pCon, char *name,
                     float newValue)
{
  SIMDriv *pDriv = (SIMDriv *) self;

  assert(self);
  assert(pCon);

  if (strcmp(name, "hardupperlim") == 0) {
    pDriv->fUpper = newValue;
    return 1;
  }
  if (strcmp(name, "hardlowerlim") == 0) {
    pDriv->fLower = newValue;
    return 1;
  }
  return 0;
}

static int SimGetTextPar(void *pData, char *name, char *textPar)
{
  snprintf(textPar, 8, "unknown");
  return 1;
}

/*---------------------------------------------------------------------------*/
void KillSIM(void *pData)
{
  return;
}

/*--------------------------------------------------------------------------*/
MotorDriver *CreateSIM(SConnection * pCon, int argc, char *argv[])
{
  TokenList *pList = NULL;
  TokenList *pCurrent;
  SIMDriv *pDriv = NULL;

  assert(pCon);

  /* check number of arguments */
  if (argc < 3) {
    SCWrite(pCon, "Insufficient numbers of arguments for SimMotor",
            eError);
    return NULL;
  }

  /* split arguments */
  pList = SplitArguments(argc, argv);
  if (!pList) {
    SCWrite(pCon, "Error parsing arguments in SimMotor", eError);
    return NULL;
  }

  /* allocate memory */
  pDriv = (SIMDriv *) malloc(sizeof(SIMDriv));
  if (!pDriv) {
    SCWrite(pCon, "Error allocating memory in SimMotor", eError);
    DeleteTokenList(pList);
    return NULL;
  }
  memset(pDriv, 0, sizeof(SIMDriv));

  /* check and enter args, first lowerLimit */
  pCurrent = pList;
  if (pCurrent->Type == eInt) {
    pDriv->fLower = (float) pCurrent->iVal;
  } else if (pCurrent->Type == eFloat) {
    pDriv->fLower = pCurrent->fVal;
  } else {
    SCWrite(pCon, "Non float argument to SimMotor", eError);
    free(pDriv);
    DeleteTokenList(pList);
    return NULL;
  }

  /* upper limit */
  pCurrent = pCurrent->pNext;
  if (pCurrent->Type == eInt) {
    pDriv->fUpper = (float) pCurrent->iVal;
  } else if (pCurrent->Type == eFloat) {
    pDriv->fUpper = pCurrent->fVal;
  } else {
    SCWrite(pCon, "Non float argument to SimMotor", eError);
    free(pDriv);
    DeleteTokenList(pList);
    return NULL;
  }

  /* failure rate */
  pCurrent = pCurrent->pNext;
  if (pCurrent->Type == eInt) {
    pDriv->fFailure = (float) pCurrent->iVal;
  } else if (pCurrent->Type == eFloat) {
    pDriv->fFailure = pCurrent->fVal;
  } else {
    SCWrite(pCon, "Non float argument to SimMotor", eError);
    free(pDriv);
    DeleteTokenList(pList);
    return NULL;
  }


  /* calculate current position, initialise func pters */
  pDriv->fPos = (pDriv->fUpper + pDriv->fLower) / 2.;
  pDriv->fTarget = pDriv->fPos;
  pDriv->name = strdup("Simulant");
  pDriv->GetPosition = GetSimPos;
  pDriv->RunTo = SimRun;
  pDriv->GetStatus = SimStat;
  pDriv->GetError = SimError;
  pDriv->TryAndFixIt = SimFix;
  /* pDriv->SetDriverPar = SimSetPar; */
  pDriv->Halt = SimHalt;
  pDriv->fSpeed = .01;
  pDriv->iTime = 0;
  pDriv->KillPrivate = KillSIM;
  pDriv->GetDriverTextPar = SimGetTextPar;

  /* check for optional speed paramter */
  pCurrent = pCurrent->pNext;
  if (pCurrent) {
    if (pCurrent->Type == eFloat) {
      pDriv->fSpeed = pCurrent->fVal;
    }
  }
  DeleteTokenList(pList);
  return (MotorDriver *) pDriv;
}