sics/moregress.c

/**
 * 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 <stdlib.h>
#include <sics.h>
#include <modriv.h>

/*===================== 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;
}