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;
  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;
}