/*----------------------------------------------------------------------------- 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 #include #include #include #include /* SICS kernel declarations */ #include "sics.h" #include "modriv.h" /* includes abstract motor defn */ #include #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 * 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 = 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("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); }