/** * This is a regression testing motor driver for SICS. * A parameter can be set which makes this driver cause * various error conditions. This can then be used to * verify and debug the working of upper level code * * copyright: see file COPYRIGHT * * Mark Koennecke, July 2007 */ #include #include #include /*===================== supported errors ======================*/ #define NONE 0 #define STARTFAIL 1 #define BADPOS 2 /* positioning problem */ #define FAIL 3 /* failure */ #define OFFPOS 4 /* off pos by .2 */ #define READFAIL 5 #define RUN 6 /* keep running; for interrupt testing */ /*=============================================================*/ typedef struct __RGMoDriv{ /* 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); /* your drivers private fields follow below */ float target; int errorType; int recover; int counter; } RGMotorDriver; /*================================================================ GetPos returns OKOK on success, HWFault on failure ------------------------------------------------------------------*/ static int RGGetPos(void *data, float *fPos){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; if(self->errorType == READFAIL){ return HWFault; } if(self->errorType > 1 && self->errorType < 6){ *fPos = self->target - .2; } else { *fPos = self->target; } return OKOK; } /*---------------------------------------------------------------- RunTo starts the motor running. Returns OKOK on success, HWfault on Errors ------------------------------------------------------------------*/ static int RGRunTo(void *data, float newValue){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; self->target = newValue; if(self->errorType == STARTFAIL){ return HWFault; } return OKOK; } /*----------------------------------------------------------------- CheckStatus queries the sattus of a running motor. Possible return values can be: HWBusy : motor still running HWFault : motor error detected HWPosFault : motor finished, but position not reached HWIdle : motor finished OK HWWarn : motor issued warning --------------------------------------------------------------------*/ static int RGCheckStatus(void *data){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; switch(self->errorType){ case BADPOS: return HWPosFault; break; case FAIL: return HWFault; break; case RUN: return HWBusy; break; } return HWIdle; } /*------------------------------------------------------------------ GetError gets more information about error which occurred *iCode is an integer error code to be used in TryFixIt as indicator buffer is a buffer for a text description of the problem iBufLen is the length of buffer --------------------------------------------------------------------*/ static void RGGetError(void *data, int *iCode, char *buffer, int iBufLen){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; *iCode = self->errorType; switch(self->errorType){ case NONE: strncpy(buffer,"No error found",iBufLen); break; case BADPOS: strncpy(buffer,"Position not reached",iBufLen); break; case FAIL: strncpy(buffer,"Hardware is mad",iBufLen); break; case STARTFAIL: strncpy(buffer,"Failed to start motor",iBufLen); break; case READFAIL: strncpy(buffer,"Failed to read motor",iBufLen); break; } } /*------------------------------------------------------------------ TryAndFixIt tries everything which is possible in software to fix a problem. iError is the error code from GetError, newValue is the target value for the motor Possible retrun values are: MOTOK : everything fixed MOTREDO : try again MOTFAIL : cannot fix this --------------------------------------------------------------------*/ static int RGFixIt(void *data, int iError, float newValue){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; if(self->recover == 1){ self->errorType = NONE; return MOTREDO; } return MOTFAIL; } /*------------------------------------------------------------------- Halt tries to stop the motor. Halt errors are ignored ---------------------------------------------------------------------*/ static int RGHalt(void *data){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; self->errorType = NONE; return 1; } /*-------------------------------------------------------------------- GetDriverPar retrieves the value of a driver parameter. Name is the name of the parameter, fValue the value when found. Returns 0 on success, 0 else -----------------------------------------------------------------------*/ static int RGGetDriverPar(void *data, char *name, float *value){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; if(strcmp(name,"errortype") == 0){ *value = (float)self->errorType; return 1; } else if (strcmp(name,"recover") == 0){ *value = self->recover; return 1; } return 0; } /*---------------------------------------------------------------------- SetDriverPar sets a driver parameter. Returns 0 on failure, 1 on success. Name is the parameter name, pCon the connection to report errors too, value the new value ------------------------------------------------------------------------*/ static int RGSetDriverPar(void *data, SConnection *pCon, char *name, float value){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; if(strcmp(name,"errortype") == 0){ self->errorType = (int)value; return 1; } else if (strcmp(name,"recover") == 0){ self->recover = (int)value; return 1; } return 0; } /*----------------------------------------------------------------------- ListDriverPar lists the names and values of driver parameters to pCon. Motorname is the name of the motor ro prefix to the listing. -------------------------------------------------------------------------*/ static void RGListDriverPar(void *data, char *motorname, SConnection *pCon){ RGMotorDriver *self = NULL; char buffer[256]; self = (RGMotorDriver *)data; snprintf(buffer,255,"%s errortype = %d", motorname, self->errorType); SCWrite(pCon,buffer,eValue); snprintf(buffer,255,"%s recover = %d", motorname, self->recover); SCWrite(pCon,buffer,eValue); } /*----------------------------------------------------------------------- KillPrivate has the task to delete possibly dynamically allocated memory in the private part of the driver structure ------------------------------------------------------------------------*/ static void RGKillPrivate(void *data){ RGMotorDriver *self = NULL; self = (RGMotorDriver *)data; } /*=======================================================================*/ MotorDriver *RGMakeMotorDriver(void) { RGMotorDriver *pNew = NULL; pNew = malloc(sizeof(RGMotorDriver)); if(pNew == NULL){ return NULL; } memset(pNew,0,sizeof(RGMotorDriver)); pNew->GetPosition = RGGetPos; pNew->RunTo = RGRunTo; pNew->GetStatus = RGCheckStatus; pNew->GetError = RGGetError; pNew->TryAndFixIt = RGFixIt; pNew->Halt = RGHalt; pNew->GetDriverPar = RGGetDriverPar; pNew->SetDriverPar = RGSetDriverPar; pNew->ListDriverPar = RGListDriverPar; pNew->KillPrivate = RGKillPrivate; pNew->fLower = -180.; pNew->fUpper = 180.; return (MotorDriver *)pNew; }