- Adapted indenation to new agreed upon system
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
562
simdriv.c
562
simdriv.c
@ -51,341 +51,297 @@
|
||||
#include "servlog.h"
|
||||
#include "splitter.h"
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static float SimRandom(void)
|
||||
{
|
||||
float fVal;
|
||||
|
||||
fVal = ( (float) rand() / (float)RAND_MAX) * 100.0;
|
||||
return fVal;
|
||||
}
|
||||
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 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;
|
||||
static int GetSimPos(void *self, float *fPos)
|
||||
{
|
||||
SIMDriv *pDriv;
|
||||
|
||||
/*
|
||||
no error checking case
|
||||
*/
|
||||
if(pDriv->fFailure < .0)
|
||||
{
|
||||
*fPos = pDriv->fPos;
|
||||
return OKOK;
|
||||
}
|
||||
assert(self);
|
||||
pDriv = (SIMDriv *) self;
|
||||
|
||||
if(SimRandom() < pDriv->fFailure)
|
||||
{
|
||||
*fPos = SimRandom();
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
/*
|
||||
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;
|
||||
static int SimRun(void *self, float fVal)
|
||||
{
|
||||
SIMDriv *pDriv;
|
||||
|
||||
/*
|
||||
no failure, no wait situation
|
||||
*/
|
||||
if(pDriv->fFailure < .0)
|
||||
{
|
||||
pDriv->fPos = fVal;
|
||||
return OKOK;
|
||||
}
|
||||
assert(self);
|
||||
pDriv = (SIMDriv *) self;
|
||||
|
||||
|
||||
/* 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;
|
||||
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;
|
||||
/*
|
||||
no failure, no wait situation
|
||||
*/
|
||||
if (pDriv->fFailure < .0) {
|
||||
pDriv->fPos = fVal;
|
||||
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;
|
||||
}
|
||||
/* 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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int SimSetPar(void *self, SConnection *pCon, char *name, float newValue)
|
||||
static int SimSetPar(void *self, SConnection * pCon, char *name,
|
||||
float newValue)
|
||||
{
|
||||
SIMDriv *pDriv = (SIMDriv *) self;
|
||||
|
||||
assert(self);
|
||||
assert(pCon);
|
||||
|
||||
if(strcmp(name,"hardupperlim") == 0)
|
||||
{
|
||||
if (strcmp(name, "hardupperlim") == 0) {
|
||||
pDriv->fUpper = newValue;
|
||||
return 1;
|
||||
}
|
||||
if(strcmp(name,"hardlowerlim") == 0)
|
||||
{
|
||||
if (strcmp(name, "hardlowerlim") == 0) {
|
||||
pDriv->fLower = newValue;
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
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;
|
||||
|
||||
/* 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)
|
||||
{
|
||||
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;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* check for optional speed paramter */
|
||||
pCurrent = pCurrent->pNext;
|
||||
if (pCurrent) {
|
||||
if (pCurrent->Type == eFloat) {
|
||||
pDriv->fSpeed = pCurrent->fVal;
|
||||
}
|
||||
}
|
||||
DeleteTokenList(pList);
|
||||
return (MotorDriver *) pDriv;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user