681 lines
20 KiB
C
681 lines
20 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 <motor.h>
|
|
#include "fortify.h"
|
|
#include "conman.h"
|
|
#include "servlog.h"
|
|
#include "splitter.h"
|
|
#include "SCinter.h"
|
|
#include "anstoutil.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
|
|
|
|
#define HARDLOWERLIM "hardlowerlim"
|
|
#define HARDUPPERLIM "hardupperlim"
|
|
#define UNITS "units"
|
|
#define SETPOS "setpos"
|
|
#define BUFFLEN 512
|
|
|
|
/* 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 */
|
|
pMotor pMot; /**< Points to logical motor object */
|
|
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 */
|
|
float home;
|
|
int iStatus;
|
|
char units[256]; /**< physical units for axis */
|
|
char long_name[256]; /**< long name of motor */
|
|
char part[256]; /**< assembly which motor belongs to */
|
|
} ASIMDriv, *pASIMDriv;
|
|
|
|
/* 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)
|
|
{
|
|
UpdateRunningTime(pDriv);
|
|
pDriv->fPos = pDriv->fTarget;
|
|
pDriv->iStatus = ASIM_IDLE;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
#if 0
|
|
static int RunComplete(ASIMDriv *pDriv)
|
|
{
|
|
UpdatePosition(pDriv);
|
|
if(ASIM_BUSY!=pDriv->iStatus) return 1; else return 0;
|
|
}
|
|
#endif
|
|
/*----------------------------------------------------------------------------*/
|
|
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;
|
|
|
|
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 = 1;
|
|
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);
|
|
/* The logical motor does this for us
|
|
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, char *motor, char *params)
|
|
{
|
|
ASIMDriv *pDriv = NULL;
|
|
time_t tD;
|
|
char *pPtr = NULL;
|
|
Tcl_Interp *interp;
|
|
|
|
assert(pCon);
|
|
interp = InterpGetTcl(pServ->pSics);
|
|
|
|
/* allocate memory */
|
|
pDriv = (ASIMDriv *)malloc(sizeof(ASIMDriv));
|
|
if(!pDriv)
|
|
{
|
|
SCWrite(pCon,"Error allocating memory in ASIM Motor",eError);
|
|
return NULL;
|
|
}
|
|
memset(pDriv,0,sizeof(ASIMDriv));
|
|
if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {
|
|
free(pDriv);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%f",&(pDriv->fLower));
|
|
if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) {
|
|
free(pDriv);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%f",&(pDriv->fUpper));
|
|
if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) {
|
|
free(pDriv);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%s",pDriv->units);
|
|
pDriv->fFailure = 0;
|
|
|
|
/* 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 = 1.0;
|
|
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 ) );
|
|
|
|
return (MotorDriver *)pDriv;
|
|
}
|
|
|
|
static void SimStrList(pASIMDriv self, char *name, SConnection *pCon){
|
|
char buffer[BUFFLEN];
|
|
|
|
snprintf(buffer, BUFFLEN, "%s.part = %s\n", name, self->part);
|
|
SCWrite(pCon, buffer, eValue);
|
|
snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, self->long_name);
|
|
SCWrite(pCon, buffer, eValue);
|
|
snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, self->units);
|
|
SCWrite(pCon, buffer, eValue);
|
|
return;
|
|
}
|
|
|
|
int SimAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pMotor pm = (pMotor) pData;
|
|
pASIMDriv self = (pASIMDriv) pm->pDriver;
|
|
|
|
if (argc > 1) {
|
|
if (strcasecmp("send", argv[1]) == 0) {
|
|
}
|
|
if (strcasecmp("units", argv[1]) == 0) {
|
|
if (argc > 2) {
|
|
strncpy(self->units, argv[2], sizeof(self->units));
|
|
self->units[sizeof(self->units) - 1] = '\0';
|
|
}
|
|
else {
|
|
char line[132];
|
|
snprintf(line, 132, "%s.units = %s", argv[0], self->units);
|
|
SCWrite(pCon, line, eValue);
|
|
}
|
|
return 1;
|
|
}
|
|
else if (strcasecmp("long_name", argv[1]) == 0) {
|
|
if (argc > 2) {
|
|
strncpy(self->long_name, argv[2], sizeof(self->long_name));
|
|
self->long_name[sizeof(self->long_name) - 1] = '\0';
|
|
}
|
|
else {
|
|
char line[132];
|
|
snprintf(line, 132, "%s.long_name = %s", argv[0], self->long_name);
|
|
SCWrite(pCon, line, eValue);
|
|
}
|
|
return 1;
|
|
}
|
|
else if (strcasecmp("part", argv[1]) == 0) {
|
|
if (argc > 2) {
|
|
strncpy(self->part, argv[2], sizeof(self->part));
|
|
self->part[sizeof(self->part) - 1] = '\0';
|
|
}
|
|
else {
|
|
char line[132];
|
|
snprintf(line, 132, "%s.part = %s", argv[0], self->part);
|
|
SCWrite(pCon, line, eValue);
|
|
}
|
|
return 1;
|
|
}
|
|
else if (strcasecmp("list", argv[1]) == 0) {
|
|
/* Handle in generic motor driver */
|
|
}
|
|
else if (strcasecmp("slist", argv[1]) == 0) {
|
|
SimStrList(self, argv[0], pCon);
|
|
return 1;
|
|
}
|
|
else if(strcasecmp(SETPOS, argv[1]) == 0) {
|
|
float oldZero, newZero, currPos, newValue;
|
|
if (self->pMot == NULL)
|
|
self->pMot = FindMotor(pServ->pSics, argv[0]);
|
|
MotorGetPar(self->pMot, "softzero", &oldZero);
|
|
if (argc > 3) {
|
|
sscanf(argv[2], "%f", &currPos);
|
|
currPos += oldZero;
|
|
sscanf(argv[3], "%f", &newValue);
|
|
}
|
|
else if (argc > 2 ){
|
|
sscanf(argv[2], "%f", &newValue);
|
|
currPos = self->pMot->fPosition;
|
|
}
|
|
else {
|
|
char buffer[BUFFLEN];
|
|
snprintf(buffer, BUFFLEN, "%s.setPos = %f\n", argv[0], oldZero);
|
|
SCWrite(pCon, buffer, eValue);
|
|
return 1;
|
|
}
|
|
newZero = (currPos - newValue);
|
|
return MotorSetPar(self->pMot, pCon, "softzero", newZero);
|
|
} else if(strcasecmp("home", argv[1]) == 0) {
|
|
if (argc > 2) {
|
|
sscanf(argv[2], "%f", &(self->home));
|
|
} else {
|
|
char line[132];
|
|
snprintf(line, 132, "%s.home = %f", argv[0], self->home);
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
}
|
|
else if(strcasecmp("reset", argv[1]) == 0) { }
|
|
else if(strcasecmp("state", argv[1]) == 0) { }
|
|
else if(strcasecmp("trace", argv[1]) == 0) { }
|
|
else if(strcasecmp("thread0", argv[1]) == 0) {
|
|
char line[132];
|
|
snprintf(line, 132, "%s.thread0 = %d", argv[0], 1);
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
} else if(strcasecmp("posit", argv[1]) == 0) {
|
|
char line[132];
|
|
snprintf(line, 132, "%s.posit = %f", argv[0], 1.0);
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
}
|
|
return MotorAction(pCon, pSics, pData, argc, argv);
|
|
}
|