Initial revision
This commit is contained in:
cvs
331
simdriv.c
Normal file
331
simdriv.c
Normal file
@ -0,0 +1,331 @@
|
||||
/*-----------------------------------------------------------------------------
|
||||
|
||||
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 baqtch-files.
|
||||
|
||||
Mark Koennecke, November 1996
|
||||
|
||||
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 "fortify.h"
|
||||
#include "conman.h"
|
||||
#include "modriv.h"
|
||||
#include "servlog.h"
|
||||
#include "splitter.h"
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static float SimRandom(void)
|
||||
{
|
||||
float fVal;
|
||||
|
||||
fVal = ( (float) rand() / (float)RAND_MAX) * 100.0;
|
||||
return fVal;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int RunComplete(SIMDriv *self)
|
||||
{
|
||||
time_t tD;
|
||||
if(time(&tD) > self->iTime)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
static int GetSimPos(void *self, float *fPos)
|
||||
{
|
||||
SIMDriv *pDriv;
|
||||
|
||||
assert(self);
|
||||
pDriv = (SIMDriv *)self;
|
||||
|
||||
if(SimRandom() < pDriv->fFailure)
|
||||
{
|
||||
*fPos = SimRandom();
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
if(RunComplete(pDriv))
|
||||
{
|
||||
*fPos = pDriv->fPos;
|
||||
}
|
||||
else /* simulate a mispositioned motor */
|
||||
{
|
||||
*fPos = pDriv->fPos - 1.;
|
||||
return OKOK;
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
static int SimRun(void *self, float fVal)
|
||||
{
|
||||
SIMDriv *pDriv;
|
||||
float fDiff;
|
||||
time_t tD;
|
||||
|
||||
assert(self);
|
||||
pDriv = (SIMDriv *)self;
|
||||
|
||||
/* calculate time for completion */
|
||||
fDiff = fVal - pDriv->fPos;
|
||||
if(fDiff < .0) fDiff = -fDiff;
|
||||
pDriv->iTime = time(&tD) + (long)(fDiff/pDriv->fSpeed);
|
||||
|
||||
/* 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;
|
||||
strncpy(error,"ERROR: HW: HahahahahahahHahahHahaha-Mmmpfff",iErrLen);
|
||||
}
|
||||
else
|
||||
{
|
||||
*iCode = 12;
|
||||
strncpy(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;
|
||||
}
|
||||
|
||||
SICSLogWrite("Simulated Motor dying randomly",eHWError);
|
||||
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;
|
||||
if(RunComplete(pDriv))
|
||||
{
|
||||
return HWIdle;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(SimRandom() < pDriv->fFailure/2)
|
||||
{
|
||||
return HWPosFault;
|
||||
}
|
||||
else if(SimRandom() < pDriv->fFailure)
|
||||
{
|
||||
return HWFault;
|
||||
}
|
||||
return HWBusy;
|
||||
}
|
||||
}
|
||||
/*--------------------------------------------------------------------------*/
|
||||
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;
|
||||
}
|
||||
|
||||
/* 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->name = strdup("Simulant");
|
||||
pDriv->GetPosition = GetSimPos;
|
||||
pDriv->RunTo = SimRun;
|
||||
pDriv->GetStatus = SimStat;
|
||||
pDriv->GetError = SimError;
|
||||
pDriv->TryAndFixIt = SimFix;
|
||||
pDriv->Halt = SimHalt;
|
||||
pDriv->ContinueAfterWarn = MoDrivXXXContinue;
|
||||
pDriv->fSpeed = .01;
|
||||
pDriv->iTime = 0;
|
||||
|
||||
/* check for optional speed paramter */
|
||||
pCurrent = pCurrent->pNext;
|
||||
if(pCurrent)
|
||||
{
|
||||
if(pCurrent->Type == eFloat)
|
||||
{
|
||||
pDriv->fSpeed = pCurrent->fVal;
|
||||
}
|
||||
}
|
||||
DeleteTokenList(pList);
|
||||
return (MotorDriver *)pDriv;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
void KillSIM(void *pData)
|
||||
{
|
||||
SIMDriv *pDriv;
|
||||
|
||||
assert(pData);
|
||||
pDriv = (SIMDriv *)pData;
|
||||
if(pDriv->name)
|
||||
free(pDriv->name);
|
||||
free(pDriv);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user