From d2e766e4b1bc2f66a3cf390518893fb17bc4518d Mon Sep 17 00:00:00 2001 From: MarkRivers Date: Tue, 21 Mar 2006 22:59:40 +0000 Subject: [PATCH] New files for simulated motor --- motorApp/MotorSimSrc/Makefile | 60 + motorApp/MotorSimSrc/devMotorSim.c | 328 ++++++ motorApp/MotorSimSrc/drvMotorSim.c | 618 ++++++++++ motorApp/MotorSimSrc/drvMotorSim.h | 13 + motorApp/MotorSimSrc/motorSimMain.cpp | 23 + motorApp/MotorSimSrc/motorSimRegister.cc | 38 + motorApp/MotorSimSrc/motorSimSupport.dbd | 4 + motorApp/MotorSimSrc/motorSimTest.db | 20 + motorApp/MotorSimSrc/motorSimTest.src | 38 + motorApp/MotorSimSrc/paramLib.c | 280 +++++ motorApp/MotorSimSrc/paramLib.h | 28 + motorApp/MotorSimSrc/route.c | 1320 ++++++++++++++++++++++ motorApp/MotorSimSrc/route.h | 69 ++ 13 files changed, 2839 insertions(+) create mode 100644 motorApp/MotorSimSrc/Makefile create mode 100644 motorApp/MotorSimSrc/devMotorSim.c create mode 100644 motorApp/MotorSimSrc/drvMotorSim.c create mode 100644 motorApp/MotorSimSrc/drvMotorSim.h create mode 100644 motorApp/MotorSimSrc/motorSimMain.cpp create mode 100644 motorApp/MotorSimSrc/motorSimRegister.cc create mode 100644 motorApp/MotorSimSrc/motorSimSupport.dbd create mode 100644 motorApp/MotorSimSrc/motorSimTest.db create mode 100644 motorApp/MotorSimSrc/motorSimTest.src create mode 100644 motorApp/MotorSimSrc/paramLib.c create mode 100644 motorApp/MotorSimSrc/paramLib.h create mode 100644 motorApp/MotorSimSrc/route.c create mode 100644 motorApp/MotorSimSrc/route.h diff --git a/motorApp/MotorSimSrc/Makefile b/motorApp/MotorSimSrc/Makefile new file mode 100644 index 00000000..44f7947f --- /dev/null +++ b/motorApp/MotorSimSrc/Makefile @@ -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 /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 + +# _registerRecordDeviceDriver.cpp will be created from .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 + diff --git a/motorApp/MotorSimSrc/devMotorSim.c b/motorApp/MotorSimSrc/devMotorSim.c new file mode 100644 index 00000000..37b6c066 --- /dev/null +++ b/motorApp/MotorSimSrc/devMotorSim.c @@ -0,0 +1,328 @@ +/* devXxxSoft.c */ +/* Example device support module */ + +#include +#include +#include +#include + +#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); +} diff --git a/motorApp/MotorSimSrc/drvMotorSim.c b/motorApp/MotorSimSrc/drvMotorSim.c new file mode 100644 index 00000000..9f8f9687 --- /dev/null +++ b/motorApp/MotorSimSrc/drvMotorSim.c @@ -0,0 +1,618 @@ +#include +#include +#include +#include +#include +#include + +#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); +} diff --git a/motorApp/MotorSimSrc/drvMotorSim.h b/motorApp/MotorSimSrc/drvMotorSim.h new file mode 100644 index 00000000..63f54342 --- /dev/null +++ b/motorApp/MotorSimSrc/drvMotorSim.h @@ -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 diff --git a/motorApp/MotorSimSrc/motorSimMain.cpp b/motorApp/MotorSimSrc/motorSimMain.cpp new file mode 100644 index 00000000..ab26c299 --- /dev/null +++ b/motorApp/MotorSimSrc/motorSimMain.cpp @@ -0,0 +1,23 @@ +/* motorMain.cpp */ +/* Author: Marty Kraimer Date: 17MAR2000 */ + +#include +#include +#include +#include +#include + +#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); +} diff --git a/motorApp/MotorSimSrc/motorSimRegister.cc b/motorApp/MotorSimSrc/motorSimRegister.cc new file mode 100644 index 00000000..d3131621 --- /dev/null +++ b/motorApp/MotorSimSrc/motorSimRegister.cc @@ -0,0 +1,38 @@ +#include +#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" + diff --git a/motorApp/MotorSimSrc/motorSimSupport.dbd b/motorApp/MotorSimSrc/motorSimSupport.dbd new file mode 100644 index 00000000..ba7b38a3 --- /dev/null +++ b/motorApp/MotorSimSrc/motorSimSupport.dbd @@ -0,0 +1,4 @@ +include "motorRecord.dbd" +device(motor,VME_IO,devMotorSim,"Motor Simulation") +driver(motorSim) +registrar(motorSimRegister) diff --git a/motorApp/MotorSimSrc/motorSimTest.db b/motorApp/MotorSimSrc/motorSimTest.db new file mode 100644 index 00000000..0e5a38ed --- /dev/null +++ b/motorApp/MotorSimSrc/motorSimTest.db @@ -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") +} + diff --git a/motorApp/MotorSimSrc/motorSimTest.src b/motorApp/MotorSimSrc/motorSimTest.src new file mode 100644 index 00000000..913ecbc9 --- /dev/null +++ b/motorApp/MotorSimSrc/motorSimTest.src @@ -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" diff --git a/motorApp/MotorSimSrc/paramLib.c b/motorApp/MotorSimSrc/paramLib.c new file mode 100644 index 00000000..ec2c33c7 --- /dev/null +++ b/motorApp/MotorSimSrc/paramLib.c @@ -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 +#include +#include +#include +#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; + } + } +} diff --git a/motorApp/MotorSimSrc/paramLib.h b/motorApp/MotorSimSrc/paramLib.h new file mode 100644 index 00000000..9866c4b1 --- /dev/null +++ b/motorApp/MotorSimSrc/paramLib.h @@ -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 diff --git a/motorApp/MotorSimSrc/route.c b/motorApp/MotorSimSrc/route.c new file mode 100644 index 00000000..c093f4a4 --- /dev/null +++ b/motorApp/MotorSimSrc/route.c @@ -0,0 +1,1320 @@ +#include +#include +#include /* For definition of the NULL pointer! */ +#include /* For definition of malloc */ +#include + +#define LOCAL static + +typedef struct path_str +{ + double dist; + double vi; + double vf; + double v2; + double t1; + double t2; + double t3; + double t4; + double T; +} path_t; + +typedef struct route_str +{ + route_pars_t pars; + route_demand_t demand; + path_t path[NUM_AXES]; + route_demand_t endp; +} route_t; + +typedef enum +{ + V2 = 1, + T = 2, + T2 = 4, + T4 = 8 +} route_unknown_t; + +#define ZERO_SIZE 2 +#define IS_ZERO(a, scale) (fabs((a)) <= fabs(ZERO_SIZE*DBL_EPSILON*(scale))) +#define DR2D (180.0/3.141592654) + + +/*+ r o u t e D e m a n d + + Function Name: routeDemand + + Function: Returns the position and velocity for a particular time on a path. + + Description: + This function returns the position and velocity at a given time of something + following the specified path. + + Call: + status = routeDemand( &path_t, t, &posn, &vel ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) path (path_t *) Path structure. + (<) t (double) Time at which to find the position and velocity + (>) demand (route_axis_demand_t *) Position and velocit at time t. + + Returns: + status (route_status_t) Always ROUTE__OK + + Author: Nick Rees + +*- + + History: +*/ + +LOCAL route_status_t routeDemand(path_t * path, double t, route_axis_demand_t * demand ) +{ + double accel1, accel2; + + + if (path->t1 != 0) accel1 = (path->v2 - path->vi) / path->t1; + else accel1 = 0; + if (path->t3 != 0) accel2 = (path->vf - path->v2) / path->t3; + else accel2 = 0; + + + if (t >= -path->t4) + { + demand->v = path->vf; + demand->p = path->vf * t; + } + else + { + t = t + path->t4; + demand->p = -path->vf * path->t4; + + if (t >= -path->t3) + { + demand->v = path->vf + accel2 * t; + demand->p += 0.5 * (demand->v + path->vf) * t; + } + else + { + t = t + path->t3; + demand->p -= 0.5 * (path->v2 + path->vf) * path->t3; + + if (t >= -path->t2) + { + demand->v = path->v2; + demand->p += path->v2 * t; + } + else + { + t = t + path->t2; + demand->p -= path->v2 * path->t2; + + if (t >= -path->t1) + { + demand->v = path->v2 + accel1 * t; + demand->p += 0.5 * (demand->v + path->v2) * t; + } + else + { + demand->v = path->vi; + demand->p += 0.5 * (path->vi + path->v2) * path->t2 + (t+path->t1)*path->vi; + } + } + } + } + +/* + + if (t <= path->t1) + { + demand->v = path->vi + accel1 * t; + demand->p = 0.5 * (path->vi + demand->v) * t; + } + else if (t <= (path->t1 + path->t2)) + { + demand->v = path->v2; + demand->p = 0.5 * (path->vi + path->v2) * path->t1 + demand->v * (t - path->t1); + } + else if (t <= (path->t1 + path->t2 + path->t3)) + { + demand->v = path->v2 + accel2 * (t - (path->t1 + path->t2)); + demand->p = (0.5 * (path->vi + path->v2) * path->t1 + path->v2 * path->t2 + + 0.5 * (path->v2 + demand->v) * (t - (path->t1 + path->t2))); + } + else + { + demand->v = path->vf; + demand->p = path->dist + path->vf * (t - path->T); + } +*/ +/* else if (t <= (path->T - path->t4)) + { + demand->v = path->vf - accel2 * (path->T - path->t4 - t); + demand->p = (path->dist - path->vf*path->t4 - + 0.5 * (path->vf + demand->v) * (path->T - path->t4 - t)); + } + else + { + demand->v = path->vf; + demand->p = path->dist + path->vf * (t - path->T); + } +*/ + return ROUTE__OK; +} + + +/*+ r o u t e F i n d W h i c h V 2 S q r t + + Function Name: routeFindWhichV2Sqrt + + Function: Returns the correct quadratic term in the path problem + + Description: + This function returns the correct solution to the second phase coast velocity + quadratic, given the linear and square root terms. + + Call: + status = routeFindWhichV2Sqrt( &path, Ai, lin_term, sqrt_term ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (!) path (path_t *) Path structure. + (<) Ai (double) Phase 1 cceleration. + (<) lin_term (double) Linear term of quadratic solution + (<) sqrt_term (route_unknown_t) Square root term of quadratic solution. + + Returns: + status (route_status_t) ROUTE__OK or ROUTE__NEGSQRT + + Author: Nick Rees +*- + History: +*/ + +LOCAL route_status_t routeFindWhichV2Sqrt( path_t * path, double Ai, + double lin_term, double sqrt_term, int unknown ) +{ + route_status_t status = ROUTE__OK; + +/* if (sqrt_term < -1e-10) + { + status = ROUTE__NEGSQRT; + } + else +*/ { + if (sqrt_term < 0) sqrt_term = 0; + + sqrt_term = sqrt( sqrt_term ); + + if (unknown == T2 ) + { + if ( Ai > 0 ) path->v2 = lin_term - sqrt_term; + else path->v2 = lin_term + sqrt_term; + } + else + { + if ( Ai > 0 ) path->v2 = lin_term + sqrt_term; + else path->v2 = lin_term - sqrt_term; + } + + } + + return status; +} + + +/*+ r o u t e F i n d P a t h + + Function Name: routeFindPath + + Function: Solves the general 4-Phase path problem given a variety of unknowns. + + Description: + + This function finds a path from a given position and velocity to a final + position and velocity via a four stage motion. + + - The first stage is a constant acceleration at +/- Amax, a fixed maximum + acceleration. + - The second stage is a coast at a constant velocity. + - The third stage is a decelleration to the final velocity at the maximum + acceleration. + - The final stage is a coast at the final velocity to the final position. + + This is equivalent to the solving the following four equations: + + distance = 0.5*(vf+v2)*t1 + v2*t2 + 0.5*(vf+v2)*t3 + vf*t4 + t2 = |(v2-vi)/Amax| + t3 = |(v2-vf)/Amax| + T = t1 + t2 + t3 + t4 + |Ai| = Amax + |Af| = Amax + + Where: + + - t1, t2, t3, and t4 are the times for each stage of the motion, + - vi, v2 and vf are the initial, stage 2 and final velocities respectively, + - T is the total time, + - Ai and Af are the initial and final accelerations respectively and + - Amax is the acceleration. + + We also are subject to the constraints t1, t2, t3, t4 > 0. + + We assume we are always given distance, |Amax|, vi, vf and |Vmax| and are + never given t1 and t3. Since we have basically 4 equations we must have 4 + unknowns, which means we also must know 2 of v2, t2, t4 and T. However, + there are sometimes 2 solutions and we also need to know the sign of Amax + (the sign of the initial acceleration). To resolve this we must use the + positivity constraints and the only way of doing this is to test all + possible cases. This makes the routine fairly messy, but I don't know a + way around this. + + In addition, it is also messy since if v2 is unknown, the answer is quadratic + in v2, and both cases in the quadratic have to be decided. + + In the case of ambiguity, the minimum to total time is taken. If two answers + have the same total time, the one with the longest final coast time is + selected. + + The mathematics are solved in the JCMT TCS design note TCS/DN/10 + + Call: + status = routeFindPath( &path, accel, unknowns ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (!) path (path_t *) Path structure. + (<) accel (double) Acceleration. + (<) unknowns (int) Bit mask of unknowns + + Returns: + status (route_status_t) Status + + Author: Nick Rees +*- + History: +*/ + +LOCAL route_status_t routeFindPath( path_t * path, + double accel, + int unknowns ) +{ + double sqrt_term, lin_term, Ai; + route_status_t status = ROUTE__OK; + + if (accel <= 0.0) return ROUTE__BADPARAM; + + switch (unknowns) + { + case ( V2 | T ): + case ( V2 | T4 ): + case ( V2 | T2 ): + { + /* Calculate: + - the minimum time spent accelerating (min_accel_time), + - the maximum time spent coasting at velocity v2 (max_t2_time) and + - the minimum distance spent on other parts of the route (min_not_t2_dist) */ + + double min_accel_time = fabs((path->vi - path->vf)/accel); + double max_t2_time = path->t2; + double vi_dist, vf_dist, min_not_t2_dist; + + switch ( unknowns & ~ V2 ) + { + case( T ): + { + min_not_t2_dist = 0.5*(path->vi + path->vf)*min_accel_time + path->vf * path->t4; + break; + } + case( T4 ): + { + min_not_t2_dist = 0.5*(path->vi + path->vf)*min_accel_time + path->vf * (path->T - path->t2 - min_accel_time); + break; + } + case (T2 ): + { + min_not_t2_dist = 0.5*(path->vi + path->vf)*min_accel_time + path->vf * path->t4; + max_t2_time = path->T - path->t4 - min_accel_time; + break; + } + default: + return ROUTE__BADPARAM; + } + + /* Calculate the two critical distances corresponding to v2=vi and v2=vf */ + vi_dist = min_not_t2_dist + path->vi * max_t2_time; + vf_dist = min_not_t2_dist + path->vf * max_t2_time; + + /* Now calculate v2 */ + if ( path->dist == vi_dist ) + { + path->v2 = path->vi; + } + else if ( path->dist == vf_dist ) + { + path->v2 = path->vf; + } + else + { + if ( ((path->dist < vi_dist) && (path->dist > vf_dist)) || + ((path->dist > vi_dist) && (path->dist < vf_dist)) ) + { + /* Velocity is intermediate between vi and vf */ + if (path->vf > path->vi) Ai = accel; + else Ai = -accel; + + /* Note: the division by max_t2_time in the following switch block + is OK since if max_t2_time = 0, then vi_dist = vf_dist and it is + impossible for path->dist to be intermediate between the two */ + + switch ( unknowns & ~V2 ) + { + case( T ): + path->v2 = (path->dist + + 0.5*(path->vi*path->vi - path->vf*path->vf)/Ai - + path->vf*path->t4 ) / max_t2_time; + break; + case( T4 ): + path->v2 = (path->dist + + 0.5*(path->vi - path->vf)*(path->vi - path->vf)/Ai - + path->vf*(path->T-path->t2)) / max_t2_time; + break; + case( T2 ): + path->v2 = ((path->dist + + 0.5*(path->vi*path->vi - path->vf*path->vf)/Ai - + path->vf*path->t4) / max_t2_time); + break; + default: + break; + } + } + else + { + /* Distance is outside range between vi_dist and vf_dist */ + + if ((path->dist < vi_dist) && (path->dist < vf_dist)) + { + Ai = -accel; + } + else /*if ((path->dist > vi_dist) && (path->dist > vf_dist))*/ + { + Ai = accel; + + } + switch ( unknowns & ~ V2 ) + { + case( T ): + lin_term = -0.5 * Ai * path->t2; + sqrt_term = 0.5 * path->t2 * Ai; + sqrt_term = (sqrt_term * sqrt_term + + 0.5 * (path->vi * path->vi + path->vf * path->vf) + + Ai * (path->dist - path->vf * path->t4)); + break; + case( T4 ): + lin_term = path->vf - 0.5 * Ai * path->t2; + sqrt_term = 0.5 * Ai * path->t2; + sqrt_term = (sqrt_term * sqrt_term + 0.5 * (path->vi - path->vf) * (path->vi - path->vf) + + Ai * (path->dist - path->vf*path->T)); + break; + case( T2 ): + lin_term = 0.5 * (Ai * (path->T-path->t4) + path->vi + path->vf); + sqrt_term = (lin_term * lin_term - + 0.5 * (path->vi * path->vi + path->vf * path->vf) - + Ai * (path->dist - path->vf * path->t4)); + break; + default: + return ROUTE__BADPARAM; + } + + /* Solve the quadratic for v2 */ + status = routeFindWhichV2Sqrt( path, + Ai, + lin_term, + sqrt_term, + (unknowns & ~V2) ); + } + } + + if (status == ROUTE__OK) + { + path->t1 = fabs( ( path->v2 - path->vi ) / accel ); + path->t3 = fabs( ( path->vf - path->v2 ) / accel ); + if (unknowns & T) path->T = path->t1 + path->t2 + path->t3 + path->t4; + if (unknowns & T4) path->t4 = path->T - (path->t1 + path->t2 + path->t3); + if (unknowns & T2) path->t2 = path->T - (path->t1 + path->t3 + path->t4); + } + break; + } + case ( T | T4 ): + case ( T | T2 ): + case ( T2| T4 ): + { + /* We know v2 - life is much easier since t1 and t3 are trivial to find ... */ + double dist; + + path->t1 = fabs( ( path->v2 - path->vi ) / accel ); + path->t3 = fabs( ( path->vf - path->v2 ) / accel ); + dist = path->dist - 0.5*((path->vi+path->v2)*path->t1 + (path->v2 + path->vf)*path->t3); + + if (unknowns & T ) + { + if ((unknowns & T4) && (path->vf != 0)) + { + path->t4 = (dist - path->v2*path->t2) / path->vf; + } + else if ((unknowns & T2) && (path->v2 != 0)) + { + path->t2 = (dist - path->vf*path->t4) / path->v2; + } + else status = ROUTE__BADPARAM; + + path->T = path->t1 + path->t2 + path->t3 + path->t4; + } + else if ( path->v2 != path->vf ) + { + path->t2 = (dist - path->vf * ( path->T - path->t1 - path->t3 ))/ (path->v2 - path->vf); + path->t4 = path->T - path->t1 - path->t2 - path->t3; + } + else status = ROUTE__BADPARAM; + + break; + } + default: + status = ROUTE__BADPARAM; + } + + /* Now do a test for positive times */ + if (status == ROUTE__OK && + (path->t1 < 0 || path->t2 < 0 || path->t3 < 0 || path->t4 < 0 || path->T < 0)) + { + if (IS_ZERO(path->t1, path->T)) path->t1 = 0; + if (IS_ZERO(path->t2, path->T)) path->t2 = 0; + if (IS_ZERO(path->t3, path->T)) path->t3 = 0; + if (IS_ZERO(path->t4, path->T)) path->t4 = 0; + + if (path->t1 < 0 || path->t2 < 0 || path->t3 < 0 || path->t4 < 0 || path->T < 0) + { + status = ROUTE__NEGTIME; + if (path->t1 < 0) path->t1 = 0; + if (path->t2 < 0) path->t2 = 0; + if (path->t3 < 0) path->t3 = 0; + if (path->t4 < 0) path->t4 = 0; + if (path->T < 0) path->T = 0; + } + } + + if (status == ROUTE__OK && + !IS_ZERO( path->T - (path->t1 + path->t2 + path->t3 + path->t4), path->T)) + { + status = ROUTE__NEGTIME; + } + +/* printf( "Path found: T=%g, t1=%g, t2=%g, t3=%g, t4=%g, vi=%g, v2=%g, vf=%g %d %d\n", + path->T, path->t1, path->t2, path->t3, path->t4, + path->vi, path->v2, path->vf, unknowns, status ); */ + + return status; +} + +/*+ r o u t e F i n d P a t h W i t h V m a x + + Function Name: routeFindPathWithVmax + + Function: Solves the 4-Phase path problem with a maximum velocity constraint. + + Description: + + This function finds a path from a given position and velocity to a final + position and velocity via a four stage motion, subject to a maximum velocity. + It uses routeFindPath to determine the paths. + + It initially sets the second stage coast time to zero and attempts to find a + path. If the maximum velocity is less than Vmax, it exits. Otherwise it + restricts the coast velocity (v2) to the maximum velocity and allows the coast + time to vary. + + + Call: + status = routeFindPathWithVmax( &path, accel, vmax, unknowns ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (!) path (path_t *) Path structure. + (<) accel (double) Acceleration. + (<) vmax (double) Maximum velocity + (<) unknowns (route_unknown_t) Unknown - either T, or t4. + + Returns: + status (route_status_t) Status from routeFindPath + + Author: Nick Rees +*- + History: +*/ + + +LOCAL route_status_t routeFindPathWithVmax(path_t * path, double Amax, double Vmax, route_unknown_t unknown) +{ + route_status_t status; + + path->t2 = 0; + status = routeFindPath(path, Amax, V2 | unknown ); + + if ((status == ROUTE__OK) && (fabs(path->v2) > Vmax)) + { + if (path->v2 >= 0.0) + path->v2 = Vmax; + else + path->v2 = -Vmax; + + status = routeFindPath( path, Amax, T2 | unknown ); + } + return status; +} + +/********************************************************************************/ +/* */ +/* External Routines */ +/* */ +/********************************************************************************/ + +/*+ r o u t e N e w + + Function Name: routeNew + + Function: Initialises the routing mechanism + + Description: + This function mallocs and initialises the internal data structures used by + the routing algorithm. + + Call: + route = routeNew( demand, params ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) demand (route_demand_t *) The inital demand for the routing system. All + subsequent routing will derive from this point. + (<) params (route_pars_t *) The initial parameters for routing. + + Returns: + route (ROUTE_ID) Either a pointer to a valid routing structure, or NULL + if an error occurs (Bad parameters or no space available) + + Author: Nick Rees + +*- + + History: +*/ + +ROUTE_ID routeNew( route_demand_t * demand, route_pars_t * params ) +{ + ROUTE_ID route = NULL; + unsigned int i, ok; + + /* Check input parameters */ + ok = (params != NULL && demand != NULL && params->Tsync >= 0); + + for (i=0; inumRoutedAxes && ok; i++) + { + int j = params->routedAxisList[i] - 1; + ok = (params->axis[j].Amax > 0 && + params->axis[j].Vmax > 0 && + params->axis[j].Vmax > fabs(demand->axis[j].v) ); + } + + /* If input parameters are OK, malloc and initialize structure */ + if ( ok && ((route = (route_t *) calloc( sizeof(route_t),1)) != NULL) ) + { + route->pars = *params; + route->endp = *demand; + route->demand = *demand; + for (i=0; inumRoutedAxes; i++) + { + int j = params->routedAxisList[i] - 1; + route->path[j].vi = demand->axis[j].v; + route->path[j].v2 = demand->axis[j].v; + route->path[j].vf = demand->axis[j].v; + } + } + + return route; +} + + +/*+ r o u t e F i n d + + Function Name: routeFind + + Function: Finds a route to a given target and returns the necessary information + + Description: + This function calculates a route from the last demand position to a new endpoint + and returns an appropriate route to this position that lies within the systems + acceleration and velocity constraints. + + Call: + status = routeFind( route, reroute, &endpoint, &nextpoint ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) Route information + (<) reroute (int) Flag set to force a total route recalculation, + including a resynchronisation to the synch + time, if required. It can have one of the following values: + ROUTE_NEW_ROUTE => Force a route recalculation, including + resychronization to the synch time. + ROUTE_NO_NEW_ROUTE => Force routing to be turned off. The next + route position is the end-point and it + will be reached at the nextpoint time. + ROUTE_CALC_ROUTE => Calculate an aceptable route to the next + point. + (!) endpoint (route_demand_t *) On input, the position and velocities in this + must be the final demand for the route. + On output, the time in the structure will be + modified to reflect the actual time the system + will be at the endpoint if the calculated route + is followed. + (!) nextpoint (route_demand_t *) On output, the time in this structure must be + the time for which the next demand is required. + This must be a later time than the previous + time this routine was called for this route. + On output, the positions and velocities in the + structure will be updated to give an appropriate + demand at the given time + + Returns: + status (route_status_t) Always ROUTE__OK + + Author: Nick Rees + +*- + + History: +*/ + +route_status_t routeFind( ROUTE_ID route, route_reroute_t reroute, route_demand_t * endp, route_demand_t * nextp ) +{ + route_status_t status = ROUTE__OK; + route_status_t ret_status = ROUTE__OK; + unsigned int long_path, short_path, i; + int old_path_ok; + + /* If no axes are routed, copy endp into nextp, and return */ + if (route->pars.numRoutedAxes == 0) + { + *nextp = *endp; + + /* Save the previous demands and endpoints */ + route->demand = *nextp; + route->endp = *endp; + return ret_status; + } + + /* If we don't want routing, set the old demand to be a coast from the current endpoint */ + if (reroute == ROUTE_NO_NEW_ROUTE) + { + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + route->demand.axis[j].v = endp->axis[j].v; + route->demand.axis[j].p = endp->axis[j].p - (nextp->T - route->demand.T)*endp->axis[j].v; + } + } + + /* First check whether the previous path is OK to use */ + old_path_ok = (reroute == ROUTE_CALC_ROUTE) && IS_ZERO( (endp->T - route->endp.T), endp->T); + for ( i = 0; i < route->pars.numRoutedAxes && old_path_ok; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + old_path_ok = (IS_ZERO( (endp->axis[j].p - route->endp.axis[j].p), 40.0) && + (fabs(endp->axis[j].v - route->endp.axis[j].v) < (route->pars.axis[j].Vmax*1.0e-10))); +/* old_path_ok = (IS_ZERO( (endp->axis[j].p - route->endp.axis[j].p), 6.0) && */ +/* IS_ZERO( (endp->axis[j].v - route->endp.axis[j].v), route->pars.axis[j].Vmax)); */ + } + + if (!old_path_ok) + { + /* Initialise the Path structures */ + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + route->path[j].dist = endp->axis[j].p - route->demand.axis[j].p; + route->path[j].vi = route->demand.axis[j].v; + route->path[j].vf = endp->axis[j].v; + route->path[j].t2 = 0.0; + route->path[j].t4 = route->pars.Tcoast; + route->path[j].T = endp->T - route->demand.T; + } + + /* Calculate whether we expect to complete this route in the next coast period */ + short_path = (reroute != ROUTE_NEW_ROUTE) && (nextp->T + route->pars.Tcoast >= endp->T); + if (short_path) + { + for (i = 0; ((i < route->pars.numRoutedAxes) && (status == ROUTE__OK)); i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + status = routeFindPathWithVmax(&route->path[j], + route->pars.axis[j].Amax, + route->pars.axis[j].Vmax, + T4 ); + } + } + + if (!short_path || (status != ROUTE__OK)) + { + /* Either we didn't find a route using the above or the time is longer */ + + long_path = 0; + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + route->path[j].t4 = route->pars.Tcoast; + status = routeFindPathWithVmax(&route->path[j], + route->pars.axis[j].Amax, + route->pars.axis[j].Vmax, + T ); + switch (status) + { + case ROUTE__OK: + case ROUTE__NEGSQRT: + case ROUTE__NEGTIME: + break; + default: + return status; + } + + if (route->path[j].T > route->path[long_path].T) long_path = j; + } + + /* Set the time for the path - synchronising it to an integral + number of Tsync units, if required */ + if (route->pars.Tsync > 0) + { + endp->T = ceil((route->demand.T + route->path[long_path].T) / + route->pars.Tsync) * route->pars.Tsync; + route->path[long_path].T = endp->T - route->demand.T; + } + else + { + endp->T = route->demand.T + route->path[long_path].T; + } + + /* Recalculate the paths for the new total path time. */ + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + route->path[j].T = route->path[long_path].T; + + /* If we are synchronising to an integral number of tic's all + paths have to be recalculated, otherwise all except the + longest path needs recaculating */ + if ( route->pars.Tsync > 0 || j != long_path ) + { + status = routeFindPath(&route->path[j], + route->pars.axis[j].Amax, + T2 | V2 ); + } + + if (status != ROUTE__OK) ret_status = status; + } + } + } + + /* Now all the paths are valid - calculate the demand position at the next time */ + for (i=0; i< route->pars.numRoutedAxes; i++) + { + int j = route->pars.routedAxisList[i] - 1; + status = routeDemand(&route->path[j], + (nextp->T - endp->T), + &(nextp->axis[j]) ); + nextp->axis[j].p += endp->axis[j].p; + if (status != ROUTE__OK) return status; + } + + /* Save the previous demands and endpoints */ + route->demand = *nextp; + route->endp = *endp; + + return ret_status; +} + +/*+ r o u t e D e l e t e + + Function Name: routeDelete + + Function: Deletes all structures associated with the given route + + Description: + This function frees the internal data structures used by the routing + algorithm. + + Call: + (void) routeNew( route ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The ID of the route to be deleted. + + Author: Nick Rees + +*- + + History: +*/ + +void routeDelete( ROUTE_ID route ) +{ + /* Check input parameters */ + free( route ); + return; +} + + +/*+ r o u t e S e t D e m a n d + + Function Name: routeSetDemand + + Function: Re-initialises the current demand of the routing mechanism + + Description: + This function re-initialises the current demand of the routing algorithm. + Note that this function is normally not required because the algorithm + assumes routing is always on, and so the current demand is maintained by + the algorithm internally. + + Call: + route = routeSetDemand( route, demand ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The route identifier. + (<) demand (route_demand_t *) The inital demand for the routing system. All + subsequent routing will derive from this point. + + Returns: + status (route_status_t) Always ROUTE__OK - you can set the demand to + anything you like. + + Author: Nick Rees + +*- + + History: +*/ + +route_status_t routeSetDemand( ROUTE_ID route, route_demand_t * demand ) +{ + route->demand = *demand; + + return ROUTE__OK; +} + +/*+ r o u t e S e t P a r a m s + + Function Name: routeSetParams + + Function: Re-initialises the routing mechanism parameters + + Description: + This function allows the user to reinitialise all the routing algorithm + parameteters on the fly. + + Call: + status = routeSetParams( route, params ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The ID of the route to be changed. + (<) params (route_pars_t *) The new routing parameters. + + Returns: + status (route_status_t) ROUTE_BADPARAMS if the times given are + negative or the velocities and accelerations are + negative or zero. Otherwise ROUTE__OK. + + Author: Nick Rees + +*- + + History: +*/ + +route_status_t routeSetParams( ROUTE_ID route, route_pars_t * params ) +{ + unsigned int i, ok; + route_status_t status = ROUTE__OK; + + /* Check input parameters */ + ok = (params != NULL && + params->Tsync >= 0 && + params->Tcoast >= 0 ); + + for (i=0; inumRoutedAxes && ok; i++) + { + int j = params->routedAxisList[i] - 1; + ok = (params->axis[j].Amax > 0 && + params->axis[j].Vmax > 0 && + params->axis[j].Vmax > fabs(route->demand.axis[j].v) ); + } + + /* If input parameters are OK, re-initialize the params structure */ + if ( ok ) + { + route->pars = *params; + } + else + { + status = ROUTE__BADPARAM; + } + return status; +} + +/*+ r o u t e G e t P a r a m s + + Function Name: routeGetParams + + Function: Returns the routing mechanism parameters + + Description: + This function allows the user to read all the routing algorithm + parameteters. + + Call: + status = routeGetParams( route, ¶ms ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The ID of the route to be interrogated. + (>) params (route_pars_t *) The current routing parameters. + + Returns: + status (route_status_t) Always ROUTE__OK. + + Author: Nick Rees + +*- + + History: +*/ + +route_status_t routeGetParams( ROUTE_ID route, route_pars_t * params ) +{ + *params = route->pars; + + return ROUTE__OK; +} + + +/*+ r o u t e G e t N u m R o u t e d A x e s + + Function Name: routeGetNumRoutedAxes + + Function: Returns the number of routed axes + + Description: + This function allows the user to determine how many axes are routed + + Call: + status = routeGetNumRoutedAxes( route, &number ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The ID of the route to be interrogated. + (>) number (unsigned int *) The number of routed axes. + + Returns: + status (route_status_t) Always ROUTE__OK. + + Author: Russell Kackley + +*- + + History: +*/ + +route_status_t routeGetNumRoutedAxes( ROUTE_ID route, unsigned int * number ) +{ + *number = route->pars.numRoutedAxes; + + return ROUTE__OK; +} + + + +/* Test routines */ + +#ifdef TEST_FIND_PATH + +#include + +int main( int * argc, char * argv[] ) +{ + int option = 1; + double value=0.0, distance; + path_t path = {0,0,0,0,0,0,0,0,0}; + double Amax = 0.0; + double Vmax = 0.0; + route_status_t status = 0; + int unknown1 = 1; + int unknown2 = 2; + int unknowns; + char line[128]; + + while ( option != 0 ) + { + printf("\n \ + Choose from: \n \ + 0 - Exit \n \ + 1 - Set Amax (%f) \n \ + 2 - Set Vmax (%f) \n \ + 3 - Set dist (%f) \n \ + 4 - Set vi (%f) \n \ + 5 - Set vf (%f) \n \ + 6 - Set v2 (%f) \n \ + 7 - Set t1 (%f) \n \ + 8 - Set t2 (%f) \n \ + 9 - Set t3 (%f) \n \ + 10 - Set t4 (%f) \n \ + 11 - Set T (%f) \n \ + 12 - Set unknown 1 (1 = v2, 2=T, 3=t2, 4=t4) (%d) \n \ + 13 - Set unknown 2 (1 = v2, 2=T, 3=t2, 4=t4) (%d) \n \ + 14 - Enter all values at once \n \ + \n \ + Enter option : ", + Amax, + Vmax, + path.dist, + path.vi, + path.vf, + path.v2, + path.t1, + path.t2, + path.t3, + path.t4, + path.T, + unknown1, unknown2 ); + + while ( gets( line ) == NULL || line[0] == '#' || (sscanf( line, "%d", &option ) != 1)); + + if (option > 0 && option < 14) + { + printf( "\n Enter value : " ); + while ( gets( line ) == NULL || line[0] == '#' || (sscanf( line, "%lf", &value ) != 1)); + + switch (option) + { + case 1: Amax = value; break; + case 2: Vmax = value; break; + case 3: path.dist = value; break; + case 4: path.vi = value; break; + case 5: path.vf = value; break; + case 6: path.v2 = value; break; + case 7: path.t1 = value; break; + case 8: path.t2 = value; break; + case 9: path.t3 = value; break; + case 10: path.t4 = value; break; + case 11: path.T = value; break; + case 12: unknown1 = (int) value; break; + case 13: unknown2 = (int) value; break; + default: break; + } + } + else if (option == 14) + { + while ( gets( line ) == NULL || line[0] == '#' || + (sscanf( line, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %d", + &Amax, &Vmax, &path.dist, + &path.vi, &path.vf, &path.v2, + &path.t1, &path.t2, &path.t3, &path.t4, &path.T, + &unknown1, &unknown2 ) != 13 )); + } + + unknowns =(0x1 << (int)(unknown1-1)) | (0x1 << (int)(unknown2-1)); + status = routeFindPath( &path, Amax, unknowns ); + distance = (path.t1 * (path.vi+path.v2) / 2 + + path.t2 * path.v2 + + path.t3 * (path.v2+path.vf) / 2 + + path.t4 * path.vf); + + printf("\n" ); + printf(" routeFindPath status = %d\n", (int) status ); + printf(" distance = %.12f\n", distance ); + printf(" distance error = %.12f\n", distance - path.dist ); + } + + return 0; +} + +#endif + +#ifdef TEST + +#include + +#define DAS2R (3.141592654/(3600.0*180.0)) + +route_demand_t route_list[] = +{ + 0.0, -100*DAS2R, 10*DAS2R, 10.0*DAS2R, 0.0, -100*DAS2R, 10*DAS2R, + 0.0, -100*DAS2R, 10*DAS2R, 0.0*DAS2R, 0.0, -100*DAS2R, 10*DAS2R, + 0.0, -100*DAS2R, 10*DAS2R, -10.0*DAS2R, 0.0, -100*DAS2R, 10*DAS2R, + 0.0, -180*3600*DAS2R, 10*DAS2R, -50*3600*DAS2R, 0.0, -180*3600*DAS2R, 10*DAS2R, + 0.0, -180*3600*DAS2R, 10*DAS2R, (-50*3600+10)*DAS2R, 0.0, -180*3600*DAS2R, 10*DAS2R, + 0.0, -180*3600*DAS2R, 10*DAS2R, (-50*3600+20)*DAS2R, 0.0, -180*3600*DAS2R, 10*DAS2R +}; + +#define NUM_DEMANDS (sizeof(route_list)/sizeof(route_demand_t)) + +route_demand_t sky = { 0.0, 0*DAS2R, 10*DAS2R, 0.0*DAS2R, 0.0 }; + +int routeTCSSim( int route_num, route_demand_t * demand, int slewing ) +{ + route_demand_t * this_route = &route_list[route_num % NUM_DEMANDS]; + int i; + + if (slewing) this_route->T = demand->T; + + for (i = 0; i < NUM_AXES; i++ ) + { + demand->axis[i].p = (sky.axis[i].p + + sky.axis[i].v * demand->T + + this_route->axis[i].p + + this_route->axis[i].v * (demand->T - this_route->T)); + demand->axis[i].v = (sky.axis[i].v + this_route->axis[i].v); + } + + return 0; +} + +int route_numbers[] = { 0, 1, 2, 3, 4, 5 }; +double route_times[] = { 10, 10, 10, 60, 60, 60 }; + +#define NUM_ROUTES (sizeof(route_numbers)/sizeof(int)) + +route_demand_t initial_demand = { 0.0, 0.0, 0.0, 0.0, 0.0 0.0, 0.0}; +route_pars_t initial_params = +{ + 3, /* numRoutedAxes */ + 1,2,3, /* routedAxisList */ + 0.05, /* Tsync */ + 1.0, /* Tcoast */ + 0.12*3600.0*DAS2R, /* Amax */ + 0.60*3600.0*DAS2R, /* Vmax */ + 0.12*3600.0*DAS2R, /* Amax */ + 0.60*3600.0*DAS2R, /* Vmax */ + 0.12*3600.0*DAS2R, /* Amax */ + 0.60*3600.0*DAS2R /* Vmax */ +}; + +#define DELTA_T (0.05); + +#include "thi_route_if.h" + +int main( int argc, char * argv[] ) +{ + ROUTE_ID route = routeNew( &initial_demand, &initial_params ); + route_demand_t demand=initial_demand; + route_demand_t next_demand=initial_demand; + double last_route_start = initial_demand.T; + int i, slewing, status, tel_step; + + for ( i = 0; i next_demand.T); + if (!slewing) demand.T = next_demand.T; + } + } + + return 0; +} + +#endif + +void routePrint( ROUTE_ID route, route_reroute_t reroute, route_demand_t * endp, route_demand_t * nextp, FILE * logfile ) +{ + int i; + + fprintf( logfile, "\nreroute %d\n", reroute); + + fprintf( logfile, "\nroute pars struct\n"); + fprintf( logfile, "Tsync, Tcoast %f %f\n", route->pars.Tsync, route->pars.Tcoast ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d Amax, Vmax %f %f\n", i, route->pars.axis[i].Amax * DR2D, route->pars.axis[i].Vmax * DR2D); + } + + fprintf( logfile, "\nroute path struct\n"); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d dist %f vi %f vf %f v2 %f t1 %f t2 %f t3 %f t4 %f T %f \n", i, + route->path[i].dist * DR2D, route->path[i].vi * DR2D, route->path[i].vf * DR2D, route->path[i].v2 * DR2D, + route->path[i].t1, route->path[i].t2, route->path[i].t3, route->path[i].t4, route->path[i].T ); + } + + fprintf( logfile, "\nroute demand struct\n"); + fprintf( logfile, "T %f\n", route->demand.T ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d p, v %f %f\n", i, route->demand.axis[i].p * DR2D, route->demand.axis[i].v * DR2D ); + } + + fprintf( logfile, "\nroute endp struct\n"); + fprintf( logfile, "T %f\n", route->endp.T ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d p, v %f %f\n", i, route->endp.axis[i].p * DR2D, route->endp.axis[i].v * DR2D ); + } + + fprintf( logfile, "\nendp struct\n"); + fprintf( logfile, "T %f\n", endp->T ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d p, v %f %f\n", i, endp->axis[i].p * DR2D, endp->axis[i].v * DR2D ); + } + + fprintf( logfile, "\nnextp struct\n"); + fprintf( logfile, "T %f\n", nextp->T ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d p, v %f %f\n", i, nextp->axis[i].p * DR2D, nextp->axis[i].v * DR2D ); + } +} + diff --git a/motorApp/MotorSimSrc/route.h b/motorApp/MotorSimSrc/route.h new file mode 100644 index 00000000..48b8d730 --- /dev/null +++ b/motorApp/MotorSimSrc/route.h @@ -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 */