73d144340d
This means that ASIM motor drivers can now be configured with the same configuration file as the DMC2280 driver. Also ensured that motors drive instantly to there target position, this streamlines testing and is useful for the script validation server. r1949 | ffr | 2007-05-10 13:15:21 +1000 (Thu, 10 May 2007) | 4 lines
661 lines
20 KiB
C
661 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 */
|
|
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)
|
|
{
|
|
float fDirn = 1.0;
|
|
UpdateRunningTime(pDriv);
|
|
pDriv->fPos = pDriv->fTarget;
|
|
pDriv->iStatus = ASIM_IDLE;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
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 = 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 = 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;
|
|
pDriv->fSpeed = 0;
|
|
|
|
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, eStatus);
|
|
snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, self->long_name);
|
|
SCWrite(pCon, buffer, eStatus);
|
|
snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, self->units);
|
|
SCWrite(pCon, buffer, eStatus);
|
|
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, eStatus);
|
|
return 1;
|
|
}
|
|
newZero = (currPos - newValue);
|
|
return MotorSetPar(self->pMot, pCon, "softzero", newZero);
|
|
}
|
|
else if(strcasecmp("reset", argv[1]) == 0) { }
|
|
else if(strcasecmp("state", argv[1]) == 0) { }
|
|
else if(strcasecmp("trace", argv[1]) == 0) { }
|
|
}
|
|
return MotorAction(pCon, pSics, pData, argc, argv);
|
|
}
|