linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
815eeefc1ce1cca6c2d537396c60c6ee8a5c98a5
sics/site_ansto/motor_asim.c
Ferdi Franceschini 348bd3aed1 SICS-46 
commands_common.tcl
Added ::motor::go_home command to run motors to their home positions.

hrpd,hipd,rsd,sans,reflectometer/commands.tcl
Initilise ::motor::is_homing_list for the ::motor::go_hom command

util/command.tcl
Allow an empty parameter list

server_config.tcl
Call the "commands" initilisation function on server init

motor_asim.c
Added the "home" parameter so we can test the go_home command

r2677 | ffr | 2008-08-14 15:00:18 +1000 (Thu, 14 Aug 2008) | 17 lines
2012-11-15 13:41:09 +11:00

670 lines
20 KiB
C
Raw Blame History

/*-----------------------------------------------------------------------------
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, 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("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) { }
}
return MotorAction(pCon, pSics, pData, argc, argv);
}
Reference in New Issue View Git Blame Copy Permalink