663 lines
19 KiB
C
663 lines
19 KiB
C
/*-----------------------------------------------------------------------------
|
|
|
|
Based on SINQ code 1996-2003 by Mark Koennecke
|
|
|
|
Modifications: Paul Hathaway April 2004
|
|
SimRun failure rate independent of polling rate
|
|
Fault condition determined at initial drive command and reported
|
|
at appropriate time:
|
|
ASIM_NO_FAULT - run to destination, report no error (OKOK,HWIdle)
|
|
ASIM_HW_FAULT - no movement, report HWFault
|
|
ASIM_POS_FAULT - run to error position, report no error (OKOK,HWIdle)
|
|
ASIM_STALL_FAULT - run to stall position, report HWFault
|
|
|
|
Copyright: See COPYRIGHT.txt
|
|
|
|
Instrument definition file usage:
|
|
Motor motname ASIM lowerlimit upperlimit failpercent [speed]
|
|
|
|
----------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <math.h>
|
|
|
|
/* SICS kernel declarations */
|
|
#include "sics.h"
|
|
#include "modriv.h" /* includes abstract motor defn */
|
|
#include "fortify.h"
|
|
#include "conman.h"
|
|
#include "servlog.h"
|
|
#include "splitter.h"
|
|
#include "SCinter.h"
|
|
|
|
/* #include "motor_driver.h" */
|
|
/* Ansto-specific simulation driver */
|
|
/* #include "sim_motor.h" */
|
|
#include "motor_asim.h"
|
|
|
|
/* ----------------------- Simulation -----------------------------------*/
|
|
/* #ifndef SICS_ANSTO_MOTOR_ASIM */
|
|
#define SICS_ANSTO_MOTOR_ASIM
|
|
#define MAX_LONG 2147483647
|
|
|
|
/* device internal status and error codes */
|
|
/* BUSY - motor in motion toward target
|
|
* IDLE - movement complete, normal condition
|
|
* FAULT - no motion, motor awaiting reset
|
|
* CRASH - no motion, unable to recover
|
|
*/
|
|
#define ASIM_BUSY 1
|
|
#define ASIM_IDLE 2
|
|
#define ASIM_FAULT 3
|
|
#define ASIM_CRASH 4
|
|
|
|
/* the first fields of ASIMDriv structure HAVE to be IDENTICAL to the
|
|
* abstract motor driver structure in modriv.h (motor_driver.h)
|
|
*/
|
|
|
|
typedef struct ___MoDriv {
|
|
/* general motor driver interface fields. REQUIRED! */
|
|
float fUpper; /* upper limit */
|
|
float fLower; /* lower limit */
|
|
char *name;
|
|
int (*GetPosition)(void *self,float *fPos);
|
|
int (*RunTo)(void *self, float fNewVal);
|
|
int (*GetStatus)(void *self);
|
|
void (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
|
|
int (*TryAndFixIt)(void *self,int iError, float fNew);
|
|
int (*Halt)(void *self);
|
|
int (*GetDriverPar)(void *self, char *name, float *value);
|
|
int (*SetDriverPar)(void *self,SConnection *pCon, char *name,
|
|
float newValue);
|
|
void (*ListDriverPar)(void *self, char *motorName, SConnection *pCon);
|
|
void (*KillPrivate)(void *self);
|
|
|
|
/* ASIM-specific fields */
|
|
float fFailure; /* percent random failures */
|
|
int iFaultType; /* used internally and for debugging */
|
|
float fSpeed;
|
|
long iDuration; /* expected duration of move (secs) */
|
|
long iTime; /* expected completion time (secs since 1969) */
|
|
long iRunningTime; /* time (sec) running current move */
|
|
long iLastPollTime;
|
|
float fPos; /* simulated current position */
|
|
float fTarget; /* simulation target position including fault */
|
|
float fDestination; /* requested final position */
|
|
int iStatus;
|
|
} ASIMDriv;
|
|
|
|
/* fault types */
|
|
#define ASIM_NO_FAULT 0
|
|
#define ASIM_HW_FAULT 1
|
|
#define ASIM_POS_FAULT 2
|
|
#define ASIM_STALL_FAULT 3
|
|
|
|
/* thresholds for determining relative proportion of fault types */
|
|
/*static const float TH_HW_FAULT = 20.0; /* ie (20-0) % of faults */
|
|
/*static const float TH_STALL_FAULT = 40.0; /* ie (40-20)% of faults */
|
|
/*static const float TH_POS_FAULT = 100.0; /* ie remaining faults */
|
|
|
|
/*static const float MIN_SPEED = 0.0001; */
|
|
|
|
#define TH_HW_FAULT 20.0
|
|
#define TH_STALL_FAULT 40.0
|
|
#define TH_POS_FAULT 100.0
|
|
#define MIN_SPEED 0.0001
|
|
|
|
/* void KillASIM(void *pData); */
|
|
static void UpdateRunningTime(ASIMDriv *);
|
|
static void UpdatePosition(ASIMDriv *);
|
|
static float SimRandom(void);
|
|
static int SimHalt(void *);
|
|
static int RunComplete(ASIMDriv *);
|
|
static int SimGetPos(void *, float *);
|
|
static int SimRun(void *, float);
|
|
static void SimError(void *self, int *iCode, char *error, int iErrLen);
|
|
static int SimFix(void *self, int iError, float fNew);
|
|
static int SimHalt(void *self);
|
|
static int SimStat(void *self);
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
static float SimRandom(void)
|
|
{
|
|
float fVal;
|
|
fVal = ( (float) rand() / (float)RAND_MAX ) * 100.0;
|
|
return fVal;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
static void UpdateRunningTime(ASIMDriv *pDriv)
|
|
{
|
|
time_t tD;
|
|
|
|
time(&tD); /* This fn valid between Jan 1970 and Jan 2038 */
|
|
if (ASIM_BUSY == pDriv->iStatus)
|
|
{
|
|
pDriv->iRunningTime += ((long)tD - pDriv->iLastPollTime);
|
|
}
|
|
pDriv->iLastPollTime = (long)tD;
|
|
}
|
|
/*----------------------------------------------------------------------------*/
|
|
static void UpdatePosition(ASIMDriv *pDriv)
|
|
{
|
|
float fDirn = 1.0;
|
|
UpdateRunningTime(pDriv);
|
|
switch (pDriv->iStatus)
|
|
{
|
|
case ASIM_BUSY:
|
|
if (pDriv->iRunningTime >= pDriv->iDuration)
|
|
{
|
|
switch (pDriv->iFaultType)
|
|
{
|
|
case ASIM_POS_FAULT: /* no break */
|
|
case ASIM_NO_FAULT:
|
|
pDriv->fPos = pDriv->fTarget;
|
|
pDriv->iStatus = ASIM_IDLE;
|
|
break;
|
|
default :
|
|
pDriv->iFaultType = ASIM_HW_FAULT; /* no break */
|
|
case ASIM_HW_FAULT:
|
|
pDriv->iStatus = ASIM_FAULT;
|
|
break;
|
|
case ASIM_STALL_FAULT:
|
|
pDriv->fPos = pDriv->fTarget;
|
|
pDriv->iStatus = ASIM_FAULT;
|
|
break;
|
|
}
|
|
}
|
|
else /* Running. Calculate intermediate position */
|
|
{
|
|
switch (pDriv->iFaultType)
|
|
{
|
|
case ASIM_POS_FAULT:
|
|
case ASIM_NO_FAULT:
|
|
case ASIM_STALL_FAULT:
|
|
if (pDriv->fTarget < pDriv->fPos) {
|
|
fDirn = -1.0;
|
|
} else fDirn = 1.0;
|
|
pDriv->fPos = pDriv->fTarget - fDirn * pDriv->fSpeed * (pDriv->iDuration - pDriv->iRunningTime);
|
|
pDriv->iStatus = ASIM_BUSY;
|
|
break;
|
|
case ASIM_HW_FAULT:
|
|
pDriv->iStatus = ASIM_FAULT;
|
|
break;
|
|
default: break; /* error: unknown fault type */
|
|
}
|
|
}
|
|
break;
|
|
case ASIM_FAULT:
|
|
case ASIM_IDLE:
|
|
case ASIM_CRASH:
|
|
default: break;
|
|
}
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
static int RunComplete(ASIMDriv *pDriv)
|
|
{
|
|
UpdatePosition(pDriv);
|
|
if(ASIM_BUSY!=pDriv->iStatus) return 1; else return 0;
|
|
}
|
|
/*----------------------------------------------------------------------------*/
|
|
static int SimGetPos(void *self, float *fPos)
|
|
{
|
|
int motCode = OKOK;
|
|
ASIMDriv *pDriv;
|
|
assert(self);
|
|
pDriv = (ASIMDriv *)self;
|
|
UpdatePosition(pDriv);
|
|
*fPos = pDriv->fPos;
|
|
switch (pDriv->iStatus)
|
|
{
|
|
case ASIM_IDLE: /* no break */
|
|
case ASIM_BUSY:
|
|
motCode = OKOK;
|
|
break;
|
|
default:
|
|
case ASIM_CRASH:
|
|
case ASIM_FAULT:
|
|
motCode = HWFault;
|
|
break;
|
|
}
|
|
return motCode;
|
|
}
|
|
/*----------------------------------------------------------------------------*/
|
|
static int SimRun(void *self, float fVal)
|
|
{
|
|
ASIMDriv *pDriv;
|
|
float fDiff;
|
|
float fFault;
|
|
int iSucceed;
|
|
float fDirection;
|
|
float fDelta;
|
|
float fDuration;
|
|
time_t tD;
|
|
|
|
assert(self);
|
|
pDriv = (ASIMDriv *)self;
|
|
|
|
switch (pDriv->iStatus)
|
|
{
|
|
case ASIM_IDLE: pDriv->iStatus = ASIM_BUSY;
|
|
case ASIM_BUSY: break;
|
|
default: pDriv->iStatus = ASIM_CRASH;
|
|
case ASIM_CRASH:
|
|
case ASIM_FAULT: return HWFault;
|
|
break;
|
|
}
|
|
|
|
/* New run command */
|
|
/* Determine desired distance and direction (fDiff >= 0.0)*/
|
|
pDriv->fDestination = fVal;
|
|
fDiff = fVal - pDriv->fPos;
|
|
if(0.0 > fDiff)
|
|
{
|
|
fDiff = -fDiff;
|
|
fDirection = -1.0;
|
|
}
|
|
else
|
|
fDirection = 1.0;
|
|
|
|
/* Determine whether fault will occur */
|
|
if ((0.0 >= pDriv->fFailure)||(SimRandom() > pDriv->fFailure))
|
|
iSucceed = 1;
|
|
else
|
|
iSucceed = 0;
|
|
|
|
/* Determine delta from actual target by fault mode */
|
|
if (0 == iSucceed)
|
|
{
|
|
fFault = SimRandom();
|
|
if ((TH_HW_FAULT > fFault)||(HWFault==pDriv->iStatus))
|
|
{ /* fail as soon as possible */
|
|
pDriv->iStatus = ASIM_FAULT;
|
|
pDriv->iFaultType = ASIM_HW_FAULT;
|
|
fDelta = -fDiff;
|
|
}
|
|
else
|
|
{
|
|
if (TH_STALL_FAULT > fFault)
|
|
{ /* stall before 80% complete */
|
|
pDriv->iFaultType = ASIM_STALL_FAULT;
|
|
fDelta = (-1.0) * fDiff * (0.2 + SimRandom()/125.0);
|
|
}
|
|
else
|
|
{ /* position fault to +/- 10% */
|
|
pDriv->iFaultType = ASIM_POS_FAULT;
|
|
fDelta = fDiff * SimRandom()/1000.0;
|
|
if (50.0 > SimRandom()) fDelta = -fDelta;
|
|
}
|
|
}
|
|
}
|
|
else /* no failure */
|
|
{
|
|
pDriv->iFaultType = ASIM_NO_FAULT;
|
|
fDelta = 0.0;
|
|
}
|
|
|
|
/* Calculate target and time using speed */
|
|
if (pDriv->fSpeed >= MIN_SPEED)
|
|
{
|
|
fDiff = fDiff + fDelta;
|
|
fDuration = fDiff/(pDriv->fSpeed);
|
|
if (fDuration > MAX_LONG)
|
|
pDriv->iDuration = 2000000000;
|
|
else
|
|
pDriv->iDuration = (long)fDuration;
|
|
}
|
|
else
|
|
{
|
|
fDiff = 0.0;
|
|
pDriv->iDuration = 0;
|
|
}
|
|
pDriv->fTarget = pDriv->fPos + fDirection * fDiff;
|
|
pDriv->iRunningTime = 0;
|
|
pDriv->iLastPollTime = time(&tD);
|
|
pDriv->iTime = tD + pDriv->iDuration;
|
|
|
|
if(ASIM_BUSY==pDriv->iStatus)
|
|
return OKOK;
|
|
else
|
|
return HWFault;
|
|
|
|
} /* SimRun */
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
/* iErrLen fixed to 131 in fn [reportAndFixError] of <motor.c>
|
|
* iCode reported to SimFix via [reportAndFixError]
|
|
*/
|
|
static void SimError(void *self, int *iCode, char *error, int iErrLen)
|
|
{
|
|
int motCode = HWFault;
|
|
ASIMDriv *pDriv;
|
|
|
|
assert(self);
|
|
pDriv = (ASIMDriv *)self;
|
|
|
|
switch (pDriv->iStatus)
|
|
{
|
|
case ASIM_IDLE: motCode = HWIdle; break;
|
|
case ASIM_BUSY: motCode = HWBusy; break;
|
|
case ASIM_CRASH:
|
|
case ASIM_FAULT:
|
|
default: motCode = HWFault; break;
|
|
}
|
|
|
|
*iCode = pDriv->iFaultType;
|
|
|
|
switch (pDriv->iFaultType)
|
|
{
|
|
case ASIM_POS_FAULT:
|
|
strncpy(error,"ERROR: ASIM Position Fault",iErrLen);
|
|
break;
|
|
case ASIM_NO_FAULT:
|
|
strncpy(error,"STATUS: ASIM No Fault",iErrLen);
|
|
break;
|
|
case ASIM_HW_FAULT:
|
|
strncpy(error,"ERROR: ASIM Motor Hardware Fault",iErrLen);
|
|
break;
|
|
case ASIM_STALL_FAULT:
|
|
strncpy(error,"ERROR: ASIM Motor Stall",iErrLen);
|
|
break;
|
|
default: /* error: unknown fault type */
|
|
strncpy(error,"ERROR: Unknown fault type",iErrLen);
|
|
break;
|
|
}
|
|
assert(strlen(error)<131);
|
|
} /* SimError */
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
static int SimFix(void *self, int iError, float fNew)
|
|
{
|
|
ASIMDriv *pDriv;
|
|
int iRet;
|
|
float fRand;
|
|
|
|
assert(self);
|
|
pDriv = (ASIMDriv *)self;
|
|
|
|
switch (pDriv->iStatus)
|
|
{
|
|
case ASIM_IDLE:
|
|
switch (pDriv->iFaultType)
|
|
{
|
|
case ASIM_POS_FAULT: return MOTOK;
|
|
case ASIM_NO_FAULT: return MOTOK;
|
|
case ASIM_HW_FAULT: return MOTFAIL;
|
|
case ASIM_STALL_FAULT: return MOTREDO;
|
|
default: return MOTFAIL; /* error: unknown fault type */
|
|
}
|
|
break;
|
|
case ASIM_BUSY: return MOTOK; break;
|
|
case ASIM_CRASH: return MOTFAIL; break;
|
|
case ASIM_FAULT:
|
|
SimHalt(self);
|
|
if(TH_HW_FAULT >= SimRandom())
|
|
{
|
|
pDriv->iStatus = ASIM_CRASH;
|
|
return MOTFAIL;
|
|
}
|
|
pDriv->iStatus = ASIM_IDLE;
|
|
pDriv->iFaultType = ASIM_NO_FAULT;
|
|
return MOTOK;
|
|
break;
|
|
default:
|
|
pDriv->iStatus = ASIM_CRASH;
|
|
pDriv->iFaultType = ASIM_HW_FAULT;
|
|
return MOTFAIL;
|
|
}
|
|
/* SICSLogWrite("Attempt fix for simulated motor",eHWError); */
|
|
} /* SimFix */
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
static int SimHalt(void *self)
|
|
{
|
|
int motCode = OKOK;
|
|
ASIMDriv *pDriv;
|
|
assert(self);
|
|
|
|
pDriv = (ASIMDriv *)self;
|
|
UpdatePosition(pDriv);
|
|
pDriv->iTime = 0;
|
|
pDriv->iDuration = 0;
|
|
pDriv->fTarget = pDriv->fPos;
|
|
pDriv->fDestination = pDriv->fPos;
|
|
switch (pDriv->iStatus)
|
|
{
|
|
case ASIM_BUSY: pDriv->iStatus = ASIM_IDLE;
|
|
case ASIM_IDLE: motCode = OKOK;
|
|
break;
|
|
default: motCode = HWFault;
|
|
break;
|
|
}
|
|
return motCode;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int SimStat(void *self)
|
|
{
|
|
int motCode = HWIdle;
|
|
ASIMDriv *pDriv;
|
|
assert(self);
|
|
pDriv = (ASIMDriv *)self;
|
|
|
|
UpdatePosition(pDriv);
|
|
switch (pDriv->iStatus)
|
|
{
|
|
case ASIM_BUSY: motCode = HWBusy; break;
|
|
case ASIM_IDLE: motCode = HWIdle; break;
|
|
default: motCode = HWFault; break;
|
|
}
|
|
return motCode;
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
static int SimSetPar(void *self, SConnection *pCon, char *name, float newValue)
|
|
{
|
|
int iSuccess = 0;
|
|
ASIMDriv *pDriv = (ASIMDriv *) self;
|
|
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
if(strcmp(name,"hardupperlim") == 0)
|
|
{
|
|
pDriv->fUpper = newValue;
|
|
iSuccess = 1;
|
|
}
|
|
if(strcmp(name,"hardlowerlim") == 0)
|
|
{
|
|
pDriv->fLower = newValue;
|
|
iSuccess = 1;
|
|
}
|
|
if(strcmp(name,"speed") == 0)
|
|
{
|
|
pDriv->fSpeed = newValue;
|
|
iSuccess = 1;
|
|
}
|
|
return iSuccess;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static int SimGetPar(void *self, char *name, float *value)
|
|
{
|
|
int iSuccess = 0;
|
|
ASIMDriv *pDriv = (ASIMDriv *) self;
|
|
assert(self);
|
|
|
|
if (0==strcmp("hardupperlim",name))
|
|
{
|
|
*value = pDriv->fUpper;
|
|
iSuccess = 1;
|
|
}
|
|
if (0==strcmp("hardlowerlim",name))
|
|
{
|
|
*value = pDriv->fLower;
|
|
iSuccess=1;
|
|
}
|
|
if (0==strcmp("speed",name))
|
|
{
|
|
*value = pDriv->fSpeed;
|
|
iSuccess=1;
|
|
}
|
|
return iSuccess;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static void SimListPar(void *self, char *motorName, SConnection *pCon)
|
|
{
|
|
ASIMDriv *pDriv = (ASIMDriv *) self;
|
|
char pBuffer[256];
|
|
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
sprintf(pBuffer,"%s.%s = %f",motorName,"hardupperlim",pDriv->fUpper);
|
|
SCWrite(pCon,pBuffer,eValue);
|
|
sprintf(pBuffer,"%s.%s = %f",motorName,"hardlowerlim",pDriv->fLower);
|
|
SCWrite(pCon,pBuffer,eValue);
|
|
sprintf(pBuffer,"%s.%s = %f",motorName,"speed",pDriv->fSpeed);
|
|
SCWrite(pCon,pBuffer,eValue);
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
static void SimKill(void *pData)
|
|
{
|
|
/*
|
|
the controller is owned by the controller object and will be
|
|
deleted when that object is removed
|
|
*/
|
|
return;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
MotorDriver *CreateASIM(SConnection *pCon, int argc, char *argv[])
|
|
{
|
|
TokenList *pList = NULL;
|
|
TokenList *pCurrent;
|
|
ASIMDriv *pDriv = NULL;
|
|
time_t tD;
|
|
|
|
assert(pCon);
|
|
|
|
/* check number of arguments */
|
|
if(argc < 3)
|
|
{
|
|
SCWrite(pCon,"Insufficient numbers of arguments for ASIM Motor",eError);
|
|
return NULL;
|
|
}
|
|
|
|
/* split arguments */
|
|
pList = SplitArguments(argc, argv);
|
|
if(!pList)
|
|
{
|
|
SCWrite(pCon,"Error parsing arguments in ASIM Motor",eError);
|
|
return NULL;
|
|
}
|
|
|
|
/* allocate memory */
|
|
pDriv = (ASIMDriv *)malloc(sizeof(ASIMDriv));
|
|
if(!pDriv)
|
|
{
|
|
SCWrite(pCon,"Error allocating memory in ASIM Motor",eError);
|
|
DeleteTokenList(pList);
|
|
return NULL;
|
|
}
|
|
memset(pDriv,0,sizeof(ASIMDriv));
|
|
|
|
/* 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,"ERROR: Expect numerical 'lower limit' at argument 1 after Motor ASIM",eError);
|
|
free(pDriv);
|
|
DeleteTokenList(pList);
|
|
return NULL;
|
|
}
|
|
|
|
/* then, 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,"ERROR: Expect numerical 'upper limit' at argument 2 after Motor ASIM",eError);
|
|
free(pDriv);
|
|
DeleteTokenList(pList);
|
|
return NULL;
|
|
}
|
|
|
|
/* thirdly, 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,"ERROR: Expect numerical 'failure rate' at argument 3 after Motor ASIM",eError);
|
|
free(pDriv);
|
|
DeleteTokenList(pList);
|
|
return NULL;
|
|
}
|
|
|
|
/* calculate current position, initialise func pters */
|
|
pDriv->fPos = (pDriv->fUpper + pDriv->fLower)/2.0;
|
|
/* pDriv->name = strdup("SICS_ANSTO_MOTOR_ASIM"); */
|
|
pDriv->name = malloc((1+strlen("SICS_ANSTO_MOTOR_ASIM"))*sizeof(char));
|
|
strcpy(pDriv->name,"SICS_ANSTO_MOTOR_ASIM");
|
|
|
|
pDriv->GetPosition = SimGetPos;
|
|
pDriv->RunTo = SimRun;
|
|
pDriv->GetStatus = SimStat;
|
|
pDriv->GetError = SimError;
|
|
pDriv->TryAndFixIt = SimFix;
|
|
pDriv->Halt = SimHalt;
|
|
pDriv->SetDriverPar = SimSetPar;
|
|
pDriv->GetDriverPar = SimGetPar;
|
|
pDriv->ListDriverPar = SimListPar;
|
|
pDriv->KillPrivate = SimKill;
|
|
|
|
/* set default parameters */
|
|
pDriv->fSpeed = 0.01;
|
|
pDriv->iTime = time(&tD);
|
|
pDriv->iFaultType = ASIM_NO_FAULT;
|
|
pDriv->iDuration = 0;
|
|
pDriv->iRunningTime = 0;
|
|
pDriv->iLastPollTime = pDriv->iTime;
|
|
pDriv->fTarget = pDriv->fPos;
|
|
pDriv->fDestination = pDriv->fPos;
|
|
pDriv->iStatus = OKOK;
|
|
|
|
srand( (unsigned)time( NULL ) );
|
|
|
|
/* check for optional speed paramter */
|
|
pCurrent = pCurrent->pNext;
|
|
if(pCurrent)
|
|
{
|
|
if(pCurrent->Type == eFloat)
|
|
{
|
|
pDriv->fSpeed = pCurrent->fVal;
|
|
if(pDriv->fSpeed < 0.0) pDriv->fSpeed = 0.0 - pDriv->fSpeed;
|
|
}
|
|
}
|
|
DeleteTokenList(pList);
|
|
return (MotorDriver *)pDriv;
|
|
}
|