/** * 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; if (self->errorType == STARTFAIL) { return HWFault; } self->target = newValue; 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: strlcpy(buffer, "No error found", iBufLen); break; case BADPOS: strlcpy(buffer, "Position not reached", iBufLen); break; case FAIL: strlcpy(buffer, "Hardware is mad", iBufLen); break; case STARTFAIL: strlcpy(buffer, "Failed to start motor", iBufLen); break; case READFAIL: strlcpy(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; } else if (strcmp(name, "hardupperlim") == 0) { self->fUpper = value; return 1; } else if (strcmp(name, "hardlowerlim") == 0) { self->fLower = 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; }