876396bb7e
- Introduced simulation mode to simdriv and simcter, i.e they never fail and finish at once. - Started defining MAD compatibility commands in Tcl - Fixed a bug in FOCUS_src which caused it to leak sockets. - Introduced setsockopt SO_REUSEADDR to all new sockets in sinqhm in order to loose the next sinqhm error.
360 lines
8.7 KiB
C
360 lines
8.7 KiB
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 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;
|
|
|
|
/*
|
|
no error checking case
|
|
*/
|
|
if(pDriv->fFailure < .0)
|
|
{
|
|
*fPos = pDriv->fPos;
|
|
return OKOK;
|
|
}
|
|
|
|
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;
|
|
|
|
/*
|
|
no failure, no wait situation
|
|
*/
|
|
if(pDriv->fFailure < .0)
|
|
{
|
|
pDriv->fPos = fVal;
|
|
return OKOK;
|
|
}
|
|
|
|
|
|
/* 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;
|
|
|
|
/*
|
|
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;
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
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);
|
|
}
|
|
|