forked from epics_driver_modules/motorBase
New files for simulated motor
This commit is contained in:
MarkRivers
@@ -0,0 +1,60 @@
|
||||
TOP=../..
|
||||
|
||||
include $(TOP)/configure/CONFIG
|
||||
#----------------------------------------
|
||||
# ADD MACRO DEFINITIONS AFTER THIS LINE
|
||||
#=============================
|
||||
|
||||
#==================================================
|
||||
# Build an IOC support library
|
||||
|
||||
LIBRARY_IOC += motorSimSupport
|
||||
|
||||
# motorRecord.h will be created from motorRecord.dbd
|
||||
# install devMotorSoft.dbd into <top>/dbd
|
||||
DBD += motorSimSupport.dbd
|
||||
|
||||
# The following are compiled and added to the Support library
|
||||
motorSimSupport_SRCS += route.c
|
||||
motorSimSupport_SRCS += paramLib.c
|
||||
motorSimSupport_SRCS += devMotorSim.c
|
||||
motorSimSupport_SRCS += drvMotorSim.c
|
||||
motorSimSupport_SRCS += motorSimRegister.cc
|
||||
|
||||
motorSimSupport_LIBS += $(EPICS_BASE_IOC_LIBS)
|
||||
|
||||
#=============================
|
||||
# build an ioc application
|
||||
|
||||
PROD_IOC = motorSim
|
||||
# motorSim.dbd will be created and installed
|
||||
DBD += motorSim.dbd
|
||||
|
||||
# motorSim.dbd will be made up from these files:
|
||||
motorSim_DBD += base.dbd
|
||||
motorSim_DBD += motorSupport.dbd
|
||||
motorSim_DBD += motorSimSupport.dbd
|
||||
|
||||
# <name>_registerRecordDeviceDriver.cpp will be created from <name>.dbd
|
||||
motorSim_SRCS += motorSim_registerRecordDeviceDriver.cpp
|
||||
motorSim_SRCS_DEFAULT += motorSimMain.cpp
|
||||
motorSim_SRCS_vxWorks += -nil-
|
||||
|
||||
# The following adds support from base/src/vxWorks
|
||||
motorSim_OBJS_vxWorks += $(EPICS_BASE_BIN)/vxComLibrary
|
||||
|
||||
motorSim_LIBS += motorSimSupport
|
||||
motorSim_LIBS += motor
|
||||
motorSim_LIBS += asyn
|
||||
|
||||
motorSim_LIBS += $(EPICS_BASE_IOC_LIBS)
|
||||
|
||||
#===========================
|
||||
|
||||
SCRIPTS += motorSimTest.boot
|
||||
DB += motorSimTest.db
|
||||
|
||||
include $(TOP)/configure/RULES
|
||||
#----------------------------------------
|
||||
# ADD RULES AFTER THIS LINE
|
||||
|
||||
@@ -0,0 +1,328 @@
|
||||
/* devXxxSoft.c */
|
||||
/* Example device support module */
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "epicsFindSymbol.h"
|
||||
#include "dbAccess.h"
|
||||
#include "recGbl.h"
|
||||
#include "registryDriverSupport.h"
|
||||
#include "drvSup.h"
|
||||
/* #include "callback.h" */
|
||||
|
||||
#include "motorRecord.h"
|
||||
#include "motor.h"
|
||||
#include "epicsExport.h"
|
||||
#include "motor_interface.h"
|
||||
|
||||
/*Create the dset for devMotor */
|
||||
static long init_record(struct motorRecord *);
|
||||
static CALLBACK_VALUE update_values(struct motorRecord *);
|
||||
static long start_trans(struct motorRecord *);
|
||||
static RTN_STATUS build_trans( motor_cmnd, double *, struct motorRecord *);
|
||||
static RTN_STATUS end_trans(struct motorRecord *);
|
||||
|
||||
struct motor_dset devMotorSim={ { 8,
|
||||
NULL,
|
||||
NULL,
|
||||
(DEVSUPFUN) init_record,
|
||||
NULL },
|
||||
update_values,
|
||||
start_trans,
|
||||
build_trans,
|
||||
end_trans };
|
||||
|
||||
epicsExportAddress(dset,devMotorSim);
|
||||
|
||||
#define SET_BIT(val,mask,set) ((set)? ((val) | (mask)): ((val) & ~(mask)))
|
||||
|
||||
/**\struct motorStatus_t
|
||||
|
||||
This structure is returned by motorAxisGetStatus and contains all
|
||||
the current information required by the motor record to indicate
|
||||
current motor status. It should probably be extended to include
|
||||
all of status information which might be useful (i.e. current
|
||||
values of all parameter values that can be set).
|
||||
|
||||
Note that the updateCount can be used to indicate whether any
|
||||
parameters have been changes since the last call to the motorAxisGetStatus routine.
|
||||
|
||||
*/
|
||||
typedef struct motorStatus_t
|
||||
{
|
||||
uint32_t status; /**< bit mask of errors and other binary information. The bit positions are in motor.h */
|
||||
int32_t position; /**< Current motor position in motor steps (if not servoing) or demand position (if servoing) */
|
||||
int32_t encoder_position; /**< Current motor position in encoder units (only available if a servo system). */
|
||||
} motorStatus_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
motorAxisDrvSET_t * drvset;
|
||||
AXIS_HDL pAxis;
|
||||
struct motorRecord * pRec;
|
||||
motorStatus_t status;
|
||||
double min_vel;
|
||||
double max_vel;
|
||||
double acc;
|
||||
motor_cmnd move_cmd;
|
||||
double param;
|
||||
int needUpdate;
|
||||
} motorPrvt_t;
|
||||
|
||||
|
||||
|
||||
void motor_callback( void * param, unsigned int nReasons, unsigned int * reasons )
|
||||
{
|
||||
struct motorRecord * pRec = (struct motorRecord *) param;
|
||||
motorPrvt_t * pPrvt = (motorPrvt_t *) pRec->dpvt;
|
||||
AXIS_HDL pAxis = pPrvt->pAxis;
|
||||
motorStatus_t * status = &(pPrvt->status);
|
||||
int i;
|
||||
|
||||
for ( i = 0; i < nReasons; i++ )
|
||||
{
|
||||
switch (reasons[i])
|
||||
{
|
||||
case motorAxisEncoderPosn:
|
||||
pPrvt->drvset->getInteger( pAxis, reasons[i], &(status->encoder_position) );
|
||||
break;
|
||||
case motorAxisPosition:
|
||||
pPrvt->drvset->getInteger( pAxis, reasons[i], &(status->position) );
|
||||
break;
|
||||
case motorAxisDirection:
|
||||
case motorAxisDone:
|
||||
case motorAxisHighHardLimit:
|
||||
case motorAxisHomeSignal:
|
||||
case motorAxisSlip:
|
||||
case motorAxisPowerOn:
|
||||
case motorAxisFollowingError:
|
||||
case motorAxisHomeEncoder:
|
||||
case motorAxisHasEncoder:
|
||||
case motorAxisProblem:
|
||||
case motorAxisMoving:
|
||||
case motorAxisHasClosedLoop:
|
||||
case motorAxisCommError:
|
||||
case motorAxisLowHardLimit:
|
||||
{
|
||||
int flag;
|
||||
int mask = (0x1<<(reasons[i]-motorAxisDirection));
|
||||
|
||||
pPrvt->drvset->getInteger( pAxis, reasons[i], &flag );
|
||||
status->status = SET_BIT( status->status, mask, flag );
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (nReasons > 0)
|
||||
{
|
||||
pPrvt->needUpdate = 1;
|
||||
scanOnce( pRec );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static long init_record(struct motorRecord * pRec )
|
||||
{
|
||||
if (pRec->out.type == VME_IO)
|
||||
{
|
||||
/* I should extract the first word of the parameter as the driver support entry table name,
|
||||
and pass the rest to the driver, but I am a bit lazy at the moment */
|
||||
motorAxisDrvSET_t * drvset = (motorAxisDrvSET_t *) registryDriverSupportFind( pRec->out.value.vmeio.parm );
|
||||
|
||||
if (drvset != NULL &&
|
||||
drvset->open != NULL )
|
||||
{
|
||||
AXIS_HDL axis = (*drvset->open)( pRec->out.value.vmeio.card,
|
||||
pRec->out.value.vmeio.signal,
|
||||
pRec->out.value.vmeio.parm );
|
||||
if (axis != NULL)
|
||||
{
|
||||
pRec->dpvt = calloc( 1, sizeof( motorPrvt_t ) );
|
||||
if (pRec->dpvt != NULL)
|
||||
{
|
||||
int i;
|
||||
motorPrvt_t * pPrvt = (motorPrvt_t *) pRec->dpvt;
|
||||
|
||||
pPrvt->drvset = drvset;
|
||||
pPrvt->pAxis = axis;
|
||||
pPrvt->pRec = pRec;
|
||||
pPrvt->move_cmd = -1;
|
||||
pPrvt->drvset->getInteger( axis, motorAxisEncoderPosn, &(pPrvt->status.encoder_position) );
|
||||
pPrvt->drvset->getInteger( axis, motorAxisPosition, &(pPrvt->status.position) );
|
||||
|
||||
/* Set the status bits */
|
||||
for ( i = motorAxisDirection; i <= motorAxisLowHardLimit; i++ )
|
||||
{
|
||||
/* Set the default to be zero for unsupported flags */
|
||||
int flag=0;
|
||||
int mask = (0x1<<(i-motorAxisDirection));
|
||||
|
||||
pPrvt->drvset->getInteger( axis, i, &flag );
|
||||
pPrvt->status.status = SET_BIT( pPrvt->status.status, mask, flag );
|
||||
}
|
||||
|
||||
(*drvset->setCallback)( axis, motor_callback, (void *) pRec );
|
||||
}
|
||||
else
|
||||
{
|
||||
if (drvset->close) (*drvset->close)( axis );
|
||||
recGblRecordError(S_drv_noDrvSup,(void *)pRec,
|
||||
"devMotor (init_record) cannot open driver support");
|
||||
return S_db_noMemory;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
recGblRecordError(S_drv_noDrvSup,(void *)pRec,
|
||||
"devMotor (init_record) cannot open device support");
|
||||
return S_db_noSupport;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
recGblRecordError(S_drv_noDrvet,(void *)pRec,
|
||||
"devMotor (init_record) cannot find device support entry table");
|
||||
return S_db_noSupport;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
recGblRecordError(S_db_badField,(void *)pRec,
|
||||
"devMotor (init_record) Illegal INP field");
|
||||
return(S_db_badField);
|
||||
}
|
||||
|
||||
return(0);
|
||||
}
|
||||
|
||||
CALLBACK_VALUE update_values(struct motorRecord * pRec)
|
||||
{
|
||||
motorPrvt_t * pPrvt = (motorPrvt_t *) pRec->dpvt;
|
||||
motorStatus_t stat = pPrvt->status;
|
||||
CALLBACK_VALUE rc;
|
||||
|
||||
rc = NOTHING_DONE;
|
||||
|
||||
if ( pPrvt->needUpdate )
|
||||
{
|
||||
pRec->rmp = stat.position;
|
||||
pRec->rep = stat.encoder_position;
|
||||
/* pRec->rvel = ptrans->vel; */
|
||||
pRec->msta = stat.status;
|
||||
rc = CALLBACK_DATA;
|
||||
pPrvt->needUpdate = 0;
|
||||
}
|
||||
return (rc);
|
||||
}
|
||||
|
||||
static long start_trans(struct motorRecord * pRec )
|
||||
{
|
||||
return(OK);
|
||||
}
|
||||
|
||||
static RTN_STATUS build_trans( motor_cmnd command,
|
||||
double * param,
|
||||
struct motorRecord * pRec )
|
||||
{
|
||||
RTN_STATUS status = OK;
|
||||
motorPrvt_t * pPrvt = (motorPrvt_t *) pRec->dpvt;
|
||||
|
||||
switch ( command )
|
||||
{
|
||||
case MOVE_ABS:
|
||||
case MOVE_REL:
|
||||
case HOME_FOR:
|
||||
case HOME_REV:
|
||||
pPrvt->move_cmd = command;
|
||||
pPrvt->param = *param;
|
||||
break;
|
||||
case LOAD_POS:
|
||||
status = (*pPrvt->drvset->setDouble)(pPrvt->pAxis, motorAxisPosition, *param );
|
||||
break;
|
||||
case SET_VEL_BASE:
|
||||
pPrvt->min_vel = *param;
|
||||
break;
|
||||
case SET_VELOCITY:
|
||||
pPrvt->max_vel = *param;
|
||||
break;
|
||||
case SET_ACCEL:
|
||||
pPrvt->acc = *param;
|
||||
break;
|
||||
case GO:
|
||||
switch (pPrvt->move_cmd)
|
||||
{
|
||||
case MOVE_ABS:
|
||||
status = (*pPrvt->drvset->move)(pPrvt->pAxis, pPrvt->param, 0,
|
||||
pPrvt->min_vel, pPrvt->max_vel, pPrvt->acc);
|
||||
break;
|
||||
case MOVE_REL:
|
||||
status = (*pPrvt->drvset->move)(pPrvt->pAxis, pPrvt->param, 1,
|
||||
pPrvt->min_vel, pPrvt->max_vel, pPrvt->acc);
|
||||
break;
|
||||
case HOME_FOR:
|
||||
status = (*pPrvt->drvset->home)(pPrvt->pAxis, pPrvt->min_vel, pPrvt->max_vel, pPrvt->acc, 1);
|
||||
break;
|
||||
case HOME_REV:
|
||||
status = (*pPrvt->drvset->home)(pPrvt->pAxis, pPrvt->min_vel, pPrvt->max_vel, pPrvt->acc, 0);
|
||||
break;
|
||||
default:
|
||||
status = ERROR;
|
||||
}
|
||||
pPrvt->move_cmd = -1;
|
||||
break;
|
||||
case SET_ENC_RATIO:
|
||||
status = (*pPrvt->drvset->setDouble)(pPrvt->pAxis, motorAxisEncoderRatio, param[0]/param[1] );
|
||||
break;
|
||||
case GET_INFO:
|
||||
pPrvt->needUpdate = 1;
|
||||
break;
|
||||
case STOP_AXIS:
|
||||
status = (*pPrvt->drvset->stop)(pPrvt->pAxis, pPrvt->acc );
|
||||
break;
|
||||
case JOG:
|
||||
status = (*pPrvt->drvset->velocityMove)(pPrvt->pAxis, pPrvt->min_vel, *param, pPrvt->acc);
|
||||
break;
|
||||
case JOG_VELOCITY:
|
||||
status = (*pPrvt->drvset->velocityMove)(pPrvt->pAxis, pPrvt->min_vel, *param, pPrvt->acc);
|
||||
break;
|
||||
case SET_PGAIN:
|
||||
status = (*pPrvt->drvset->setDouble)(pPrvt->pAxis, motorAxisPGain, *param );
|
||||
break;
|
||||
case SET_IGAIN:
|
||||
status = (*pPrvt->drvset->setDouble)(pPrvt->pAxis, motorAxisIGain, *param );
|
||||
break;
|
||||
case SET_DGAIN:
|
||||
status = (*pPrvt->drvset->setDouble)(pPrvt->pAxis, motorAxisIGain, *param );
|
||||
break;
|
||||
case ENABLE_TORQUE:
|
||||
status = (*pPrvt->drvset->setInteger)(pPrvt->pAxis, motorAxisClosedLoop, 1 );
|
||||
break;
|
||||
case DISABL_TORQUE:
|
||||
status = (*pPrvt->drvset->setInteger)(pPrvt->pAxis, motorAxisClosedLoop, 0 );
|
||||
break;
|
||||
case SET_HIGH_LIMIT:
|
||||
status = (*pPrvt->drvset->setDouble)(pPrvt->pAxis, motorAxisHighLimit, *param );
|
||||
break;
|
||||
case SET_LOW_LIMIT:
|
||||
status = (*pPrvt->drvset->setDouble)(pPrvt->pAxis, motorAxisLowLimit, *param );
|
||||
break;
|
||||
default:
|
||||
status = ERROR;
|
||||
}
|
||||
|
||||
return(status);
|
||||
}
|
||||
|
||||
static RTN_STATUS end_trans(struct motorRecord * pRec )
|
||||
{
|
||||
/* motorPrvt_t * pPrvt = (motorPrvt_t *) pRec->dpvt; */
|
||||
|
||||
/* callbackRequestProcessCallback( &(pPrvt->callback), priorityLow, pRec ); */
|
||||
return(OK);
|
||||
}
|
||||
@@ -0,0 +1,618 @@
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdarg.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "drvMotorSim.h"
|
||||
#include "paramLib.h"
|
||||
|
||||
#include "epicsFindSymbol.h"
|
||||
#include "epicsTime.h"
|
||||
#include "epicsThread.h"
|
||||
#include "epicsMutex.h"
|
||||
#include "ellLib.h"
|
||||
|
||||
#include "drvSup.h"
|
||||
#include "epicsExport.h"
|
||||
#define DEFINE_MOTOR_PROTOTYPES 1
|
||||
#include "motor_interface.h"
|
||||
|
||||
#include "route.h"
|
||||
|
||||
motorAxisDrvSET_t motorSim =
|
||||
{
|
||||
20,
|
||||
motorAxisReport, /**< Standard EPICS driver report function (optional) */
|
||||
motorAxisInit, /**< Standard EPICS dirver initialisation function (optional) */
|
||||
motorAxisSetLog, /**< Defines an external logging function (optional) */
|
||||
motorAxisOpen, /**< Driver open function */
|
||||
motorAxisClose, /**< Driver close function */
|
||||
motorAxisSetCallback, /**< Provides a callback function the driver can call when the status updates */
|
||||
motorAxisPrimitive, /**< Passes a controller dependedent string */
|
||||
motorAxisSetDouble, /**< Pointer to function to set a double value */
|
||||
motorAxisSetInteger, /**< Pointer to function to set an integer value */
|
||||
motorAxisGetDouble, /**< Pointer to function to get a double value */
|
||||
motorAxisGetInteger, /**< Pointer to function to get an integer value */
|
||||
motorAxisHome, /**< Pointer to function to execute a more to reference or home */
|
||||
motorAxisMove, /**< Pointer to function to execute a position move */
|
||||
motorAxisVelocityMove, /**< Pointer to function to execute a velocity mode move */
|
||||
motorAxisStop /**< Pointer to function to stop motion */
|
||||
};
|
||||
|
||||
epicsExportAddress(drvet, motorSim);
|
||||
|
||||
typedef enum { none, positionMove, velocityMove, homeReverseMove, homeForwardsMove } moveType;
|
||||
|
||||
/* typedef struct motorAxis * AXIS_ID; */
|
||||
|
||||
typedef struct motorAxisHandle
|
||||
{
|
||||
AXIS_HDL pNext;
|
||||
AXIS_HDL pPrev;
|
||||
int card;
|
||||
int axis;
|
||||
ROUTE_ID route;
|
||||
PARAMS params;
|
||||
route_reroute_t reroute;
|
||||
route_demand_t endpoint;
|
||||
route_demand_t nextpoint;
|
||||
double hiHardLimit;
|
||||
double lowHardLimit;
|
||||
double enc_offset;
|
||||
double home;
|
||||
int homing;
|
||||
epicsTimeStamp tLast;
|
||||
epicsMutexId axisMutex;
|
||||
} motorAxis;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
AXIS_HDL pFirst;
|
||||
epicsThreadId motorThread;
|
||||
motorAxisLogFunc print;
|
||||
epicsTimeStamp now;
|
||||
} motorSim_t;
|
||||
|
||||
static int motorSimLogMsg( const motorAxisSev_t severity, const char *pFormat, ...);
|
||||
#define PRINT (drv.print)
|
||||
#define INFO motorAxisErrInfo
|
||||
#define WARNING motorAxisErrMinor
|
||||
#define ERROR motorAxisErrMajor
|
||||
#define FATAL motorAxisErrFatal
|
||||
|
||||
static motorSim_t drv={ NULL, NULL, motorSimLogMsg, { 0, 0 } };
|
||||
|
||||
#define MAX(a,b) ((a)>(b)? (a): (b))
|
||||
#define MIN(a,b) ((a)<(b)? (a): (b))
|
||||
|
||||
static void motorAxisReportAxis( AXIS_HDL pAxis, int level )
|
||||
{
|
||||
PRINT( INFO, "Found driver for motorSim card %d, axis %d", pAxis->card, pAxis->axis );
|
||||
|
||||
if (level > 0)
|
||||
{
|
||||
double lowSoftLimit=0.0;
|
||||
double hiSoftLimit=0.0;
|
||||
|
||||
PRINT( INFO, "Current position = %f, velocity = %f at current time: %f",
|
||||
pAxis->nextpoint.axis[0].p,
|
||||
pAxis->nextpoint.axis[0].v,
|
||||
pAxis->nextpoint.T );
|
||||
PRINT( INFO, "Destination posn = %f, velocity = %f at desination time: %f",
|
||||
pAxis->endpoint.axis[0].p,
|
||||
pAxis->endpoint.axis[0].v,
|
||||
pAxis->endpoint.T );
|
||||
|
||||
PRINT( INFO, "Hard limits: %f, %f", pAxis->lowHardLimit, pAxis->hiHardLimit );
|
||||
paramGetDouble( pAxis->params, motorAxisHighLimit, &hiSoftLimit );
|
||||
paramGetDouble( pAxis->params, motorAxisLowLimit, &lowSoftLimit );
|
||||
PRINT( INFO, "Soft limits: %f, %f", lowSoftLimit, hiSoftLimit );
|
||||
|
||||
if (pAxis->homing) PRINT( INFO, "Currently homing axis" );
|
||||
|
||||
paramDump( pAxis->params );
|
||||
}
|
||||
}
|
||||
|
||||
static void motorAxisReport( int level )
|
||||
{
|
||||
AXIS_HDL pAxis;
|
||||
|
||||
for ( pAxis=drv.pFirst; pAxis != NULL; pAxis = pAxis->pNext ) motorAxisReportAxis( pAxis, level );
|
||||
}
|
||||
|
||||
|
||||
static int motorAxisInit( void )
|
||||
{
|
||||
return MOTOR_AXIS_OK;
|
||||
}
|
||||
|
||||
static int motorAxisSetLog( motorAxisLogFunc logFunc )
|
||||
{
|
||||
if (logFunc == NULL) drv.print=motorSimLogMsg;
|
||||
else drv.print = logFunc;
|
||||
|
||||
return MOTOR_AXIS_OK;
|
||||
}
|
||||
|
||||
static AXIS_HDL motorAxisOpen( int card, int axis, char * param )
|
||||
{
|
||||
AXIS_HDL pAxis;
|
||||
|
||||
for ( pAxis=drv.pFirst;
|
||||
pAxis != NULL && (card != pAxis->card || axis != pAxis->axis );
|
||||
pAxis = pAxis->pNext ) {}
|
||||
|
||||
return pAxis;
|
||||
}
|
||||
|
||||
static int motorAxisClose( AXIS_HDL pAxis )
|
||||
{
|
||||
return MOTOR_AXIS_OK;
|
||||
}
|
||||
|
||||
static int motorAxisGetInteger( AXIS_HDL pAxis, motorAxisParam_t function, int * value )
|
||||
{
|
||||
if (pAxis == NULL) return MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
return paramGetInteger( pAxis->params, (paramIndex) function, value );
|
||||
}
|
||||
}
|
||||
|
||||
static int motorAxisGetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double * value )
|
||||
{
|
||||
if (pAxis == NULL) return MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
return paramGetDouble( pAxis->params, (paramIndex) function, value );
|
||||
}
|
||||
}
|
||||
|
||||
static int motorAxisSetCallback( AXIS_HDL pAxis, motorAxisCallbackFunc callback, void * param )
|
||||
{
|
||||
if (pAxis == NULL) return MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
return paramSetCallback( pAxis->params, callback, param );
|
||||
}
|
||||
}
|
||||
|
||||
static int motorAxisPrimitive( AXIS_HDL pAxis, int length, char * string )
|
||||
{
|
||||
return MOTOR_AXIS_OK;
|
||||
}
|
||||
|
||||
static int motorAxisSetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double value )
|
||||
{
|
||||
int status = MOTOR_AXIS_OK;
|
||||
|
||||
if (pAxis == NULL) return MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
switch (function)
|
||||
{
|
||||
case motorAxisPosition:
|
||||
{
|
||||
pAxis->enc_offset = value - pAxis->nextpoint.axis[0].p;
|
||||
PRINT( INFO, "Set card %d, axis %d to position %f", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisEncoderRatio:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d to enc. ratio to %f", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisLowLimit:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d low limit to %f", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisHighLimit:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d high limit to %f", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisPGain:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d pgain to %f", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisIGain:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d igain to %f", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisDGain:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d dgain to %f", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisClosedLoop:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d closed loop to %s", pAxis->card, pAxis->axis, (value!=0?"ON":"OFF") );
|
||||
break;
|
||||
}
|
||||
default:
|
||||
status = MOTOR_AXIS_ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
if (status != MOTOR_AXIS_ERROR ) status = paramSetDouble( pAxis->params, function, value );
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
static int motorAxisSetInteger( AXIS_HDL pAxis, motorAxisParam_t function, int value )
|
||||
{
|
||||
int status = MOTOR_AXIS_OK;
|
||||
|
||||
if (pAxis == NULL) return MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
switch (function)
|
||||
{
|
||||
case motorAxisPosition:
|
||||
{
|
||||
pAxis->enc_offset = (double) value - pAxis->nextpoint.axis[0].p;
|
||||
PRINT( INFO, "Set card %d, axis %d to position %d", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisLowLimit:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d low limit to %d", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisHighLimit:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d high limit to %d", pAxis->card, pAxis->axis, value );
|
||||
break;
|
||||
}
|
||||
case motorAxisClosedLoop:
|
||||
{
|
||||
PRINT( INFO, "Set card %d, axis %d closed loop to %s", pAxis->card, pAxis->axis, (value?"ON":"OFF") );
|
||||
break;
|
||||
}
|
||||
default:
|
||||
status = MOTOR_AXIS_ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
if (status != MOTOR_AXIS_ERROR ) status = paramSetInteger( pAxis->params, function, value );
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
static int motorAxisMove( AXIS_HDL pAxis, double position, int relative, double min_velocity, double max_velocity, double acceleration )
|
||||
{
|
||||
if (pAxis == NULL) return MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
if (relative) position += pAxis->endpoint.axis[0].p + pAxis->enc_offset;
|
||||
|
||||
/* Check to see if in hard limits */
|
||||
if ((pAxis->nextpoint.axis[0].p >= pAxis->hiHardLimit && position > pAxis->nextpoint.axis[0].p) ||
|
||||
(pAxis->nextpoint.axis[0].p <= pAxis->lowHardLimit && position < pAxis->nextpoint.axis[0].p) ) return MOTOR_AXIS_ERROR;
|
||||
else if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK)
|
||||
{
|
||||
route_pars_t pars;
|
||||
|
||||
pAxis->endpoint.axis[0].p = position - pAxis->enc_offset;
|
||||
pAxis->endpoint.axis[0].v = 0.0;
|
||||
routeGetParams( pAxis->route, &pars );
|
||||
if (max_velocity != 0) pars.axis[0].Vmax = fabs(max_velocity);
|
||||
if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration);
|
||||
routeSetParams( pAxis->route, &pars );
|
||||
paramSetInteger( pAxis->params, motorAxisDone, 0 );
|
||||
paramSetInteger( pAxis->params, motorAxisMoving, 1 );
|
||||
epicsMutexUnlock( pAxis->axisMutex );
|
||||
|
||||
PRINT( INFO, "Set card %d, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f",
|
||||
pAxis->card, pAxis->axis, position, min_velocity, max_velocity, acceleration );
|
||||
}
|
||||
}
|
||||
return MOTOR_AXIS_OK;
|
||||
}
|
||||
|
||||
static int motorAxisVelocity( AXIS_HDL pAxis, double velocity, double acceleration )
|
||||
{
|
||||
route_pars_t pars;
|
||||
double deltaV = velocity - pAxis->nextpoint.axis[0].v;
|
||||
|
||||
/* Check to see if in hard limits */
|
||||
if ((pAxis->nextpoint.axis[0].p > pAxis->hiHardLimit && velocity > 0) ||
|
||||
(pAxis->nextpoint.axis[0].p < pAxis->lowHardLimit && velocity < 0) ) return MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
routeGetParams( pAxis->route, &pars );
|
||||
if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration);
|
||||
routeSetParams( pAxis->route, &pars );
|
||||
|
||||
pAxis->endpoint.axis[0].v = velocity;
|
||||
pAxis->endpoint.axis[0].p = pAxis->nextpoint.axis[0].p + 0.5*deltaV * fabs(deltaV/pars.axis[0].Amax);
|
||||
paramSetInteger( pAxis->params, motorAxisDone, 0 );
|
||||
paramSetInteger( pAxis->params, motorAxisMoving, 1 );
|
||||
pAxis->reroute = ROUTE_NEW_ROUTE;
|
||||
}
|
||||
return MOTOR_AXIS_OK;
|
||||
}
|
||||
|
||||
static int motorAxisHome( AXIS_HDL pAxis, double min_velocity, double max_velocity, double acceleration, int forwards )
|
||||
{
|
||||
int status = MOTOR_AXIS_ERROR;
|
||||
|
||||
if (pAxis == NULL) status = MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
status = motorAxisVelocity( pAxis, (forwards? max_velocity: -max_velocity), acceleration );
|
||||
pAxis->homing = 1;
|
||||
|
||||
PRINT( INFO, "Set card %d, axis %d to home %s, min vel=%f, max_vel=%f, accel=%f",
|
||||
pAxis->card, pAxis->axis, (forwards?"FORWARDS":"REVERSE"), min_velocity, max_velocity, acceleration );
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
static int motorAxisVelocityMove( AXIS_HDL pAxis, double min_velocity, double velocity, double acceleration )
|
||||
{
|
||||
int status = MOTOR_AXIS_ERROR;
|
||||
|
||||
if (pAxis == NULL) status = MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK)
|
||||
{
|
||||
status = motorAxisVelocity( pAxis, velocity, acceleration );
|
||||
epicsMutexUnlock( pAxis->axisMutex );
|
||||
PRINT( INFO, "Set card %d, axis %d move with velocity of %f, accel=%f",
|
||||
pAxis->card, pAxis->axis, velocity, acceleration );
|
||||
}
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
static int motorAxisProfileMove( AXIS_HDL pAxis, int npoints, double positions[], double times[], int relative, int trigger )
|
||||
{
|
||||
return MOTOR_AXIS_ERROR;
|
||||
}
|
||||
|
||||
static int motorAxisTriggerProfile( AXIS_HDL pAxis )
|
||||
{
|
||||
return MOTOR_AXIS_ERROR;
|
||||
}
|
||||
|
||||
static int motorAxisStop( AXIS_HDL pAxis, double acceleration )
|
||||
{
|
||||
if (pAxis == NULL) return MOTOR_AXIS_ERROR;
|
||||
else
|
||||
{
|
||||
motorAxisVelocity( pAxis, 0.0, acceleration );
|
||||
|
||||
PRINT( INFO, "Set card %d, axis %d to stop with accel=%f",
|
||||
pAxis->card, pAxis->axis, acceleration );
|
||||
}
|
||||
return MOTOR_AXIS_OK;
|
||||
}
|
||||
|
||||
/**\defgroup motorSimTask Routines to implement the motor axis simulation task
|
||||
@{
|
||||
*/
|
||||
|
||||
/** Process one iteration of an axis
|
||||
|
||||
This routine takes a single axis and propogates its motion forward a given amount
|
||||
of time.
|
||||
|
||||
\param pAxis [in] Pointer to axis information.
|
||||
\param delta [in] Time in seconds to propogate motion forwards.
|
||||
|
||||
\return Integer indicating 0 (MOTOR_AXIS_OK) for success or non-zero for failure.
|
||||
*/
|
||||
|
||||
static void motorSimProcess( AXIS_HDL pAxis, double delta )
|
||||
{
|
||||
double lastpos = pAxis->nextpoint.axis[0].p;
|
||||
|
||||
pAxis->nextpoint.T += delta;
|
||||
routeFind( pAxis->route, pAxis->reroute, &(pAxis->endpoint), &(pAxis->nextpoint) );
|
||||
/* if (pAxis->reroute == ROUTE_NEW_ROUTE) routePrint( pAxis->route, pAxis->reroute, &(pAxis->endpoint), &(pAxis->nextpoint), stdout ); */
|
||||
pAxis->reroute = ROUTE_CALC_ROUTE;
|
||||
|
||||
/* No, do a limits check */
|
||||
if (pAxis->homing &&
|
||||
((lastpos - pAxis->home) * (pAxis->nextpoint.axis[0].p - pAxis->home)) <= 0)
|
||||
{
|
||||
/* Homing and have crossed the home sensor - return to home */
|
||||
pAxis->homing = 0;
|
||||
pAxis->reroute = ROUTE_NEW_ROUTE;
|
||||
pAxis->endpoint.axis[0].p = pAxis->home;
|
||||
pAxis->endpoint.axis[0].v = 0.0;
|
||||
}
|
||||
if ( pAxis->nextpoint.axis[0].p > pAxis->hiHardLimit && pAxis->nextpoint.axis[0].v > 0 )
|
||||
{
|
||||
if (pAxis->homing) motorAxisVelocity( pAxis, -pAxis->endpoint.axis[0].v, 0.0 );
|
||||
else
|
||||
{
|
||||
pAxis->reroute = ROUTE_NEW_ROUTE;
|
||||
pAxis->endpoint.axis[0].p = pAxis->hiHardLimit;
|
||||
pAxis->endpoint.axis[0].v = 0.0;
|
||||
}
|
||||
}
|
||||
else if (pAxis->nextpoint.axis[0].p < pAxis->lowHardLimit && pAxis->nextpoint.axis[0].v < 0)
|
||||
{
|
||||
if (pAxis->homing) motorAxisVelocity( pAxis, -pAxis->endpoint.axis[0].v, 0.0 );
|
||||
else
|
||||
{
|
||||
pAxis->reroute = ROUTE_NEW_ROUTE;
|
||||
pAxis->endpoint.axis[0].p = pAxis->lowHardLimit;
|
||||
pAxis->endpoint.axis[0].v = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
paramSetDouble( pAxis->params, motorAxisPosition, (pAxis->nextpoint.axis[0].p+pAxis->enc_offset) );
|
||||
paramSetDouble( pAxis->params, motorAxisEncoderPosn, (pAxis->nextpoint.axis[0].p+pAxis->enc_offset) );
|
||||
paramSetInteger( pAxis->params, motorAxisDirection, (pAxis->nextpoint.axis[0].v > 0) );
|
||||
paramSetInteger( pAxis->params, motorAxisDone, (pAxis->nextpoint.axis[0].v == 0) );
|
||||
paramSetInteger( pAxis->params, motorAxisHighHardLimit, (pAxis->nextpoint.axis[0].p >= pAxis->hiHardLimit) );
|
||||
paramSetInteger( pAxis->params, motorAxisHomeSignal, (pAxis->nextpoint.axis[0].p == pAxis->home) );
|
||||
paramSetInteger( pAxis->params, motorAxisMoving, (pAxis->nextpoint.axis[0].v != 0) );
|
||||
paramSetInteger( pAxis->params, motorAxisLowHardLimit, (pAxis->nextpoint.axis[0].p <= pAxis->lowHardLimit) );
|
||||
}
|
||||
|
||||
#define DELTA 0.1
|
||||
static void motorSimTask( motorSim_t * pDrv )
|
||||
{
|
||||
epicsTimeStamp now;
|
||||
double delta;
|
||||
AXIS_HDL pAxis;
|
||||
|
||||
while ( 1 )
|
||||
{
|
||||
/* Get a new timestamp */
|
||||
epicsTimeGetCurrent( &now );
|
||||
delta = epicsTimeDiffInSeconds( &now, &(pDrv->now) );
|
||||
pDrv->now = now;
|
||||
|
||||
if ( delta > (DELTA/4.0) && delta <= (4.0*DELTA) )
|
||||
{
|
||||
/* A reasonable time has elapsed, it's not a time step in the clock */
|
||||
|
||||
for (pAxis = pDrv->pFirst; pAxis != NULL; pAxis = pAxis->pNext )
|
||||
{
|
||||
if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK)
|
||||
{
|
||||
motorSimProcess( pAxis, delta );
|
||||
paramCallCallback( pAxis->params );
|
||||
epicsMutexUnlock( pAxis->axisMutex );
|
||||
}
|
||||
}
|
||||
}
|
||||
epicsThreadSleep( DELTA );
|
||||
}
|
||||
}
|
||||
|
||||
static int motorSimCreateAxis( motorSim_t * pDrv, int card, int axis, double lowLimit, double hiLimit, double home )
|
||||
{
|
||||
AXIS_HDL pAxis;
|
||||
AXIS_HDL * ppLast = &(pDrv->pFirst);
|
||||
|
||||
for ( pAxis = pDrv->pFirst;
|
||||
pAxis != NULL &&
|
||||
! ((pAxis->card == card) && (pAxis->axis == axis));
|
||||
pAxis = pAxis->pNext )
|
||||
{
|
||||
ppLast = &(pAxis->pNext);
|
||||
}
|
||||
|
||||
if ( pAxis == NULL)
|
||||
{
|
||||
pAxis = (AXIS_HDL) calloc( 1, sizeof(motorAxis) );
|
||||
if (pAxis != NULL)
|
||||
{
|
||||
route_pars_t pars;
|
||||
|
||||
pars.numRoutedAxes = 1;
|
||||
pars.routedAxisList[0] = 1;
|
||||
pars.Tsync = 0.0;
|
||||
pars.Tcoast = 0.0;
|
||||
pars.axis[0].Amax = 1.0;
|
||||
pars.axis[0].Vmax = 1.0;
|
||||
|
||||
pAxis->endpoint.T = 0;
|
||||
pAxis->endpoint.axis[0].p = 0;
|
||||
pAxis->endpoint.axis[0].v = 0;
|
||||
|
||||
if ((pAxis->route = routeNew( &(pAxis->endpoint), &pars )) != NULL &&
|
||||
(pAxis->params = paramCreate( MOTOR_AXIS_NUM_PARAMS )) != NULL &&
|
||||
(pAxis->axisMutex = epicsMutexCreate( )) != NULL )
|
||||
{
|
||||
pAxis->card = card;
|
||||
pAxis->axis = axis;
|
||||
pAxis->hiHardLimit = hiLimit;
|
||||
pAxis->lowHardLimit = lowLimit;
|
||||
pAxis->home = home;
|
||||
*ppLast = pAxis;
|
||||
PRINT( INFO, "Created motor for card %d, signal %d OK", card, axis );
|
||||
}
|
||||
else
|
||||
{
|
||||
if (pAxis->route != NULL) routeDelete( pAxis->route );
|
||||
if (pAxis->params != NULL) paramDestroy( pAxis->params );
|
||||
if (pAxis->axisMutex != NULL) epicsMutexDestroy( pAxis->axisMutex );
|
||||
free ( pAxis );
|
||||
pAxis = NULL;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
free ( pAxis );
|
||||
pAxis = NULL;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PRINT( WARNING, "Motor for card %d, signal %d already exists", card, axis );
|
||||
return MOTOR_AXIS_ERROR;
|
||||
}
|
||||
|
||||
if (pAxis == NULL)
|
||||
{
|
||||
PRINT( ERROR, "Cannot create motor for card %d, signal %d", card, axis );
|
||||
return MOTOR_AXIS_ERROR;
|
||||
}
|
||||
|
||||
return MOTOR_AXIS_OK;
|
||||
}
|
||||
|
||||
|
||||
void motorSimCreate( int card, int axis, double lowLimit, double hiLimit, double home, int nCards, int nAxes )
|
||||
{
|
||||
int i;
|
||||
int j;
|
||||
|
||||
if (nCards < 1) nCards = 1;
|
||||
if (nAxes < 1 ) nAxes = 1;
|
||||
|
||||
PRINT( INFO,
|
||||
"Creating motor simulator: card: %d, axis: %d, hi: %f, low %f, home: %f, ncards: %d, naxis: %d",
|
||||
card, axis, hiLimit, lowLimit, home, nCards, nAxes );
|
||||
|
||||
if (drv.motorThread==NULL)
|
||||
{
|
||||
drv.motorThread = epicsThreadCreate( "motorSimThread",
|
||||
epicsThreadPriorityLow,
|
||||
epicsThreadGetStackSize(epicsThreadStackMedium),
|
||||
(EPICSTHREADFUNC) motorSimTask, (void *) &drv );
|
||||
|
||||
if (drv.motorThread == NULL)
|
||||
{
|
||||
PRINT( FATAL, "Cannot start motor simulation thread" );
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
for ( i = card; i < card+nCards; i++ )
|
||||
{
|
||||
for (j = axis; j < axis+nAxes; j++ )
|
||||
{
|
||||
motorSimCreateAxis( &drv, i, j, lowLimit, hiLimit, home );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int motorSimLogMsg( const motorAxisSev_t severity, const char *pFormat, ...)
|
||||
{
|
||||
|
||||
va_list pvar;
|
||||
int nchar;
|
||||
|
||||
va_start(pvar, pFormat);
|
||||
nchar = vfprintf(stdout,pFormat,pvar);
|
||||
va_end (pvar);
|
||||
printf("\n");
|
||||
return(nchar);
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef DRV_MOTOR_SIM_H
|
||||
#define DRV_MOTOR_SIM_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
void motorSimCreate( int card, int axis, double hiLimit, double lowLimit, double home, int nCards, int nAxes );
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
@@ -0,0 +1,23 @@
|
||||
/* motorMain.cpp */
|
||||
/* Author: Marty Kraimer Date: 17MAR2000 */
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "epicsExit.h"
|
||||
#include "epicsThread.h"
|
||||
#include "iocsh.h"
|
||||
|
||||
int main(int argc,char *argv[])
|
||||
{
|
||||
if(argc>=2) {
|
||||
iocsh(argv[1]);
|
||||
epicsThreadSleep(.2);
|
||||
}
|
||||
iocsh(NULL);
|
||||
epicsExit(0);
|
||||
return(0);
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
#include <iocsh.h>
|
||||
#include "drvMotorSim.h"
|
||||
#include "epicsExport.h"
|
||||
|
||||
extern "C" {
|
||||
|
||||
static const iocshArg motorSimCreateArg0 = { "Card", iocshArgInt};
|
||||
static const iocshArg motorSimCreateArg1 = { "Signal", iocshArgInt};
|
||||
static const iocshArg motorSimCreateArg2 = { "High limit", iocshArgDouble};
|
||||
static const iocshArg motorSimCreateArg3 = { "Low limit", iocshArgDouble};
|
||||
static const iocshArg motorSimCreateArg4 = { "Home position", iocshArgDouble};
|
||||
static const iocshArg motorSimCreateArg5 = { "Num cards", iocshArgInt};
|
||||
static const iocshArg motorSimCreateArg6 = { "Num signals", iocshArgInt};
|
||||
|
||||
static const iocshArg *const motorSimCreateArgs[] = {
|
||||
&motorSimCreateArg0,
|
||||
&motorSimCreateArg1,
|
||||
&motorSimCreateArg2,
|
||||
&motorSimCreateArg3,
|
||||
&motorSimCreateArg4,
|
||||
&motorSimCreateArg5,
|
||||
&motorSimCreateArg6,
|
||||
};
|
||||
static const iocshFuncDef motorSimCreateDef ={"motorSimCreate",7,motorSimCreateArgs};
|
||||
|
||||
static void motorSimCreateCallFunc(const iocshArgBuf *args)
|
||||
{
|
||||
motorSimCreate(args[0].ival, args[1].ival, args[2].dval, args[3].dval, args[4].dval, args[5].ival, args[6].ival);
|
||||
}
|
||||
|
||||
void motorSimRegister(void)
|
||||
{
|
||||
iocshRegister(&motorSimCreateDef, motorSimCreateCallFunc);
|
||||
}
|
||||
epicsExportRegistrar(motorSimRegister);
|
||||
|
||||
} // extern "C"
|
||||
|
||||
@@ -0,0 +1,4 @@
|
||||
include "motorRecord.dbd"
|
||||
device(motor,VME_IO,devMotorSim,"Motor Simulation")
|
||||
driver(motorSim)
|
||||
registrar(motorSimRegister)
|
||||
@@ -0,0 +1,20 @@
|
||||
grecord(motor,"$(DEVICE):test")
|
||||
{
|
||||
field(DESC,"Motor Simulation test")
|
||||
field(DTYP,"Motor Simulation")
|
||||
field(VELO,"1")
|
||||
field(VBAS,"0")
|
||||
field(VMAX,"1")
|
||||
field(HVEL,"1")
|
||||
field(ACCL,"1")
|
||||
field(BDST,"0.01")
|
||||
field(BVEL,"0.1")
|
||||
field(BACC,"0.1")
|
||||
field(OUT,"#C0 S0 @motorSim")
|
||||
field(MRES,"0.001")
|
||||
field(PREC,"5")
|
||||
field(EGU,"mm")
|
||||
field(DHLM,"30")
|
||||
field(DLLM,"-30")
|
||||
}
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
#!$(INSTALL)/bin/$(ARCH)/motorSim
|
||||
|
||||
## You may have to change test to something else
|
||||
## everywhere it appears in this file
|
||||
|
||||
cd "$(INSTALL)"
|
||||
|
||||
# Load binaries on architectures that need to do so.
|
||||
# VXWORKS_ONLY, LINUX_ONLY and RTEMS_ONLY are macros that resolve
|
||||
# to a comment symbol on architectures that are not the current
|
||||
# build architecture, so they can be used liberally to do architecture
|
||||
# specific things. Alternatively, you can include an architecture
|
||||
# specific file.
|
||||
$(VXWORKS_ONLY)ld < bin/$(ARCH)/test.munch
|
||||
|
||||
## This drvTS initializer is needed if the IOC has a hardware event system
|
||||
#TSinit
|
||||
|
||||
## Register all support components
|
||||
dbLoadDatabase("dbd/motorSim.dbd")
|
||||
motorSim_registerRecordDeviceDriver(pdbbase)
|
||||
|
||||
## Load record instances
|
||||
dbLoadRecords("db/motorSimTest.db","DEVICE=npr78")
|
||||
#dbLoadRecords("db/dbExample2.db","user=npr78,no=1,scan=1 second")
|
||||
#dbLoadRecords("db/dbExample2.db","user=npr78,no=2,scan=2 second")
|
||||
#dbLoadRecords("db/dbExample2.db","user=npr78,no=3,scan=5 second")
|
||||
#dbLoadRecords("db/dbSubExample.db","user=npr78")
|
||||
|
||||
## Set this to see messages from mySub
|
||||
#mySubDebug 1
|
||||
|
||||
motorSimCreate( 0, 0, -32000, 32000, 0, 1, 1 )
|
||||
|
||||
iocInit()
|
||||
|
||||
## Start any sequence programs
|
||||
#seq sncExample,"user=npr78Host"
|
||||
@@ -0,0 +1,280 @@
|
||||
/**\mainpage Simple parameter system that can be used for EPICS motor drivers.
|
||||
|
||||
This is a simple parameter system designed to make parameter storage and
|
||||
callback notification simpler for EPICS motor drivers. The calls are compatible
|
||||
with the EPICS motor API, so a number of the routines in a motor driver can
|
||||
be simple pass-through routines calling this library. For an example, see the
|
||||
drvMotorSim.c code.
|
||||
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include "paramLib.h"
|
||||
|
||||
typedef enum { paramUndef, paramDouble, paramInt } paramType;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
paramType type;
|
||||
union
|
||||
{
|
||||
double dval;
|
||||
int ival;
|
||||
} data;
|
||||
} paramVal;
|
||||
|
||||
typedef struct paramList
|
||||
{
|
||||
paramIndex nvals;
|
||||
int * flags;
|
||||
paramIndex * set_flags;
|
||||
paramVal * vals;
|
||||
paramCallback callback;
|
||||
void * param;
|
||||
} paramList;
|
||||
|
||||
/** Deletes a parameter system created by paramCreate.
|
||||
|
||||
Allocates data structures for a parameter system with the given number of
|
||||
parameters. Parameters stored in the system are are accessed via and index number
|
||||
ranging from 0 to the number of values minus 1.
|
||||
|
||||
\param params [in] Pointer to PARAM handle returned by paramCreate.
|
||||
|
||||
\return void.
|
||||
*/
|
||||
void paramDestroy( PARAMS params )
|
||||
{
|
||||
if (params->flags != NULL) free( params->flags );
|
||||
if (params->set_flags != NULL) free( params->set_flags );
|
||||
if (params->vals != NULL) free( params->vals );
|
||||
free( params );
|
||||
params = NULL;
|
||||
}
|
||||
|
||||
/** Creates a parameter system with a given number of values
|
||||
|
||||
Allocates data structures for a parameter system with the given number of
|
||||
parameters. Parameters stored in the system are are accessed via and index number
|
||||
ranging from 0 to the number of values minus 1.
|
||||
|
||||
\param nvals [in] Number of parameters.
|
||||
|
||||
\return Handle to be passed to other parameter system routines or NULL if system cannot be created.
|
||||
*/
|
||||
PARAMS paramCreate( paramIndex nvals )
|
||||
{
|
||||
PARAMS params = (PARAMS) calloc( 1, sizeof(paramList ));
|
||||
|
||||
if ( nvals > 0 &&
|
||||
(params != NULL) &&
|
||||
((params->flags = (int *) calloc( nvals, sizeof(int))) != NULL ) &&
|
||||
((params->set_flags = (paramIndex *) calloc( nvals, sizeof(paramIndex))) != NULL ) &&
|
||||
((params->vals = (paramVal *) calloc( nvals, sizeof(paramVal)) ) != NULL ) )
|
||||
{
|
||||
params->nvals = nvals;
|
||||
}
|
||||
else
|
||||
{
|
||||
paramDestroy( params );
|
||||
}
|
||||
|
||||
return params;
|
||||
}
|
||||
|
||||
|
||||
/** Sets the value of an integer parameter.
|
||||
|
||||
Sets the value of the parameter associated with a given index to an integer value.
|
||||
|
||||
\param params [in] Pointer to PARAM handle returned by paramCreate.
|
||||
\param index [in] Index number of the parameter.
|
||||
\param value [in] Value to be assigned to the parameter.
|
||||
|
||||
\return Integer indicating 0 (PARAM_OK) for success or non-zero for index out of range.
|
||||
*/
|
||||
int paramSetInteger( PARAMS params, paramIndex index, int value )
|
||||
{
|
||||
int status = PARAM_ERROR;
|
||||
|
||||
if (index >= 0 && index < params->nvals)
|
||||
{
|
||||
if ( params->vals[index].type != paramInt ||
|
||||
params->vals[index].data.ival != value )
|
||||
{
|
||||
params->flags[index] = 1;
|
||||
params->vals[index].type = paramInt;
|
||||
params->vals[index].data.ival = value;
|
||||
}
|
||||
status = PARAM_OK;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
/** Sets the value of a double parameter.
|
||||
|
||||
Sets the value of the parameter associated with a given index to a double value.
|
||||
|
||||
\param params [in] Pointer to PARAM handle returned by paramCreate.
|
||||
\param index [in] Index number of the parameter.
|
||||
\param value [in] Value to be assigned to the parameter.
|
||||
|
||||
\return Integer indicating 0 (PARAM_OK) for success or non-zero for index out of range.
|
||||
*/
|
||||
int paramSetDouble( PARAMS params, paramIndex index, double value )
|
||||
{
|
||||
int status = PARAM_ERROR;
|
||||
|
||||
if (index >=0 && index < params->nvals)
|
||||
{
|
||||
if ( params->vals[index].type != paramDouble ||
|
||||
params->vals[index].data.dval != value )
|
||||
{
|
||||
params->flags[index] = 1;
|
||||
params->vals[index].type = paramDouble;
|
||||
params->vals[index].data.dval = value;
|
||||
}
|
||||
status = PARAM_OK;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
/** Gets the value of an integer parameter.
|
||||
|
||||
Returns the value of the parameter associated with a given index as an integer value.
|
||||
|
||||
\param params [in] Pointer to PARAM handle returned by paramCreate.
|
||||
\param index [in] Index number of the parameter.
|
||||
\param value [out] Value of the parameter as a integer.
|
||||
|
||||
\return Integer indicating 0 (PARAM_OK) for success or non-zero for index out of range.
|
||||
*/
|
||||
int paramGetInteger( PARAMS params, paramIndex index, int * value )
|
||||
{
|
||||
int status = PARAM_OK;
|
||||
|
||||
if (index >= 0 && index < params->nvals)
|
||||
{
|
||||
switch (params->vals[index].type)
|
||||
{
|
||||
case paramDouble: *value = (int) floor(params->vals[index].data.dval+0.5); break;
|
||||
case paramInt: *value = params->vals[index].data.ival; break;
|
||||
default: status = 0;
|
||||
}
|
||||
}
|
||||
else status = PARAM_ERROR;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/** Gets the value of a double parameter.
|
||||
|
||||
Gets the value of the parameter associated with a given index as a double value.
|
||||
|
||||
\param params [in] Pointer to PARAM handle returned by paramCreate.
|
||||
\param index [in] Index number of the parameter.
|
||||
\param value [out] Value of the parameter as a double.
|
||||
|
||||
\return Integer indicating 0 (PARAM_OK) for success or non-zero for index out of range.
|
||||
*/
|
||||
int paramGetDouble( PARAMS params, paramIndex index, double * value )
|
||||
{
|
||||
int status = PARAM_OK;
|
||||
|
||||
if (index >= 0 && index < params->nvals)
|
||||
{
|
||||
switch (params->vals[index].type)
|
||||
{
|
||||
case paramDouble: *value = params->vals[index].data.dval; break;
|
||||
case paramInt: *value = (double) params->vals[index].data.ival; break;
|
||||
default: status = 0;
|
||||
}
|
||||
}
|
||||
else status = PARAM_ERROR;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/** Sets a callback routing to call when parameters change
|
||||
|
||||
This sets the value of a routine which is called whenever the user calls paramCallCallback and
|
||||
a value in the parameter system has been changed.
|
||||
|
||||
\param params [in] Pointer to PARAM handle returned by paramCreate.
|
||||
\param callback [in] Index number of the parameter. This must be a routine that
|
||||
takes three parameters and returns void.
|
||||
The first paramet is the pointer passed as the third parameter to this routine.
|
||||
The second is an integer indicating the number of parameters that have changed.
|
||||
The third is an array of parameter indices that indicates the parameters that
|
||||
have changed.
|
||||
\param param [in] Pointer to a paramemter to be passed to the callback routine.
|
||||
|
||||
\return Integer indicating 0 (PARAM_OK) for success or non-zero for index out of range.
|
||||
*/
|
||||
int paramSetCallback( PARAMS params, paramCallback callback, void * param )
|
||||
{
|
||||
params->callback = callback;
|
||||
params->param = param;
|
||||
return PARAM_OK;
|
||||
}
|
||||
|
||||
/** Calls the callback routine indicating which parameters have changed.
|
||||
|
||||
This routine should be called whenever you have changed a number of parameters and wish
|
||||
to notify someone (via the callback routine) that they have changed.
|
||||
|
||||
\param params [in] Pointer to PARAM handle returned by paramCreate.
|
||||
|
||||
\return void
|
||||
*/
|
||||
void paramCallCallback( PARAMS params )
|
||||
{
|
||||
unsigned int i;
|
||||
int nFlags=0;
|
||||
|
||||
for (i = 0; i < params->nvals; i++)
|
||||
{
|
||||
if (params->flags[i])
|
||||
{
|
||||
params->set_flags[nFlags] = i;
|
||||
nFlags++;
|
||||
params->flags[i] = 0;
|
||||
}
|
||||
|
||||
}
|
||||
if ( nFlags > 0 && params->callback != NULL ) params->callback( params->param, nFlags, params->set_flags );
|
||||
}
|
||||
|
||||
/** Prints the current values in the parameter system to stdout
|
||||
|
||||
This routine prints all the values in the parameter system to stdout.
|
||||
If the values are currently undefined, this is noted.
|
||||
|
||||
\param params [in] Pointer to PARAM handle returned by paramCreate.
|
||||
|
||||
\return void
|
||||
*/
|
||||
void paramDump( PARAMS params )
|
||||
{
|
||||
unsigned int i;
|
||||
|
||||
printf( "Number of parameters is: %d\n", params->nvals );
|
||||
for (i =0; i < params->nvals; i++)
|
||||
{
|
||||
switch (params->vals[i].type)
|
||||
{
|
||||
case paramDouble:
|
||||
printf( "Parameter %d is a double, value %f\n", i, params->vals[i].data.dval );
|
||||
break;
|
||||
case paramInt:
|
||||
printf( "Parameter %d is an integer, value %d\n", i, params->vals[i].data.ival );
|
||||
break;
|
||||
default:
|
||||
printf( "Parameter %d is undefined\n", i );
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#ifndef PARAM_LIB_H
|
||||
#define PARAM_LIB_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define PARAM_OK (0)
|
||||
#define PARAM_ERROR (-1)
|
||||
|
||||
typedef unsigned int paramIndex;
|
||||
typedef struct paramList * PARAMS;
|
||||
typedef void (*paramCallback)( void *, unsigned int, unsigned int * );
|
||||
|
||||
PARAMS paramCreate( paramIndex nvals );
|
||||
void paramDestroy( PARAMS params );
|
||||
int paramSetInteger( PARAMS params, paramIndex index, int value );
|
||||
int paramSetDouble( PARAMS params, paramIndex index, double value );
|
||||
void paramCallCallback( PARAMS params );
|
||||
int paramGetInteger( PARAMS params, paramIndex index, int * value );
|
||||
int paramGetDouble( PARAMS params, paramIndex index, double * value );
|
||||
int paramSetCallback( PARAMS params, paramCallback callback, void * param );
|
||||
void paramDump( PARAMS params );
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,69 @@
|
||||
#ifndef __INCrouteLibh
|
||||
#define __INCrouteLibh
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define NUM_AXES 3
|
||||
|
||||
typedef enum
|
||||
{
|
||||
ROUTE_CALC_ROUTE = 0,
|
||||
ROUTE_NEW_ROUTE = 1,
|
||||
ROUTE_NO_NEW_ROUTE = 2
|
||||
} route_reroute_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
ROUTE__OK = 0,
|
||||
ROUTE__BADROUTE = 1,
|
||||
ROUTE__BADPARAM = 2,
|
||||
ROUTE__NEGSQRT = 3,
|
||||
ROUTE__NEGTIME = 4
|
||||
} route_status_t;
|
||||
|
||||
typedef struct route_axis_demand_str
|
||||
{
|
||||
double p; /* Demand position for axis at a given time */
|
||||
double v; /* Demand velocity for axis at a given time */
|
||||
} route_axis_demand_t;
|
||||
|
||||
typedef struct route_demand_str
|
||||
{
|
||||
double T; /* Time at which demand is valid */
|
||||
route_axis_demand_t axis[NUM_AXES];
|
||||
} route_demand_t;
|
||||
|
||||
typedef struct route_axis_pars_str
|
||||
{
|
||||
double Amax; /* Maximum acceleration for this axis */
|
||||
double Vmax; /* Maximum velocity for this axis */
|
||||
} route_axis_pars_t;
|
||||
|
||||
typedef struct route_pars_str
|
||||
{
|
||||
unsigned int numRoutedAxes; /* Number of axes to be routed */
|
||||
int routedAxisList[NUM_AXES]; /* List of the axes to be routed */
|
||||
double Tsync; /* Synchronisation period for routing */
|
||||
double Tcoast; /* End of route coast time for all axes */
|
||||
route_axis_pars_t axis[NUM_AXES];
|
||||
} route_pars_t;
|
||||
|
||||
typedef struct route_str * ROUTE_ID;
|
||||
|
||||
ROUTE_ID routeNew( route_demand_t * initialDemand, route_pars_t * initial_parameters );
|
||||
route_status_t routeFind( ROUTE_ID, route_reroute_t, route_demand_t * end_demand, route_demand_t * next_demand );
|
||||
void routePrint( ROUTE_ID route, route_reroute_t reroute, route_demand_t * endp, route_demand_t * nextp, FILE * logfile );
|
||||
void routeDelete( ROUTE_ID );
|
||||
|
||||
route_status_t routeSetDemand( ROUTE_ID, route_demand_t * demand );
|
||||
route_status_t routeSetParams( ROUTE_ID, route_pars_t * parameters );
|
||||
route_status_t routeGetParams( ROUTE_ID, route_pars_t * parameters );
|
||||
route_status_t routeGetNumRoutedAxes( ROUTE_ID route, unsigned int * number );
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __INCrouteLibh */
|
||||
Reference in New Issue
Block a user