- Switched motor to hdb
- Changes to Hipadaba - Added project to histogram memory code - Started regression testing code - Added hill climbing as optimization method to optimise
This commit is contained in:
koennecke
258
moregress.c
Normal file
258
moregress.c
Normal file
@ -0,0 +1,258 @@
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user