linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Modified the ANSTO simulated motor driver so that it now accepts a list of named parameters, and supports the extra metadata parameters needed for generating hipadaba interfaces.

Browse Source 
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
This commit is contained in:
Ferdi Franceschini
2007-05-10 13:15:21 +10:00
committed by Douglas Clowes
parent cb7d9fc8ae
commit 73d144340d
3 changed files with 142 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files

278
site_ansto/motor_asim.c
View File

@@ -26,11 +26,13 @@
/* 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 */
@@ -53,11 +55,17 @@
#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 {
typedef struct ___MoDriv {
/* general motor driver interface fields. REQUIRED! */
float fUpper; /* upper limit */
float fLower; /* lower limit */
@@ -75,6 +83,7 @@
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;
@@ -86,7 +95,10 @@
float fTarget; /* simulation target position including fault */
float fDestination; /* requested final position */
int iStatus;
} ASIMDriv;
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
@@ -95,11 +107,11 @@
#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 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; */
//static const float MIN_SPEED = 0.0001; */
#define TH_HW_FAULT 20.0
#define TH_STALL_FAULT 40.0
@@ -143,54 +155,8 @@ static int SimStat(void *self);
{
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;
}
pDriv->fPos = pDriv->fTarget;
pDriv->iStatus = ASIM_IDLE;
}
/*---------------------------------------------------------------------------*/
static int RunComplete(ASIMDriv *pDriv)
@@ -452,7 +418,7 @@ static int SimStat(void *self);
/*-----------------------------------------------------------------------*/
static int SimSetPar(void *self, SConnection *pCon, char *name, float newValue)
{
int iSuccess = 0;
int iSuccess = 1;
ASIMDriv *pDriv = (ASIMDriv *) self;
assert(self);
@@ -509,13 +475,14 @@ static void SimListPar(void *self, char *motorName, SConnection *pCon)
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);
*/
}
/*---------------------------------------------------------------------------*/
@@ -529,94 +496,41 @@ static void SimListPar(void *self, char *motorName, SConnection *pCon)
}
/*--------------------------------------------------------------------------*/
MotorDriver *CreateASIM(SConnection *pCon, int argc, char *argv[])
MotorDriver *CreateASIM(SConnection *pCon, char *motor, char *params)
{
TokenList *pList = NULL;
TokenList *pCurrent;
ASIMDriv *pDriv = NULL;
time_t tD;
char *pPtr = NULL;
Tcl_Interp *interp;
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;
}
interp = InterpGetTcl(pServ->pSics);
/* 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));
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;
/* 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"); */
@@ -644,19 +558,103 @@ MotorDriver *CreateASIM(SConnection *pCon, int argc, char *argv[])
pDriv->fTarget = pDriv->fPos;
pDriv->fDestination = pDriv->fPos;
pDriv->iStatus = OKOK;
pDriv->fSpeed = 0;
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;
}
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);
}