From c711e15100b0a029932fc49e278825aee5f7775c Mon Sep 17 00:00:00 2001 From: kpetersn Date: Mon, 15 Apr 2019 14:29:45 -0500 Subject: [PATCH] Removed MotorSimSrc; Added motorMotorSim submodule --- .gitmodules | 3 + iocBoot/iocSim/Makefile | 9 - iocBoot/iocSim/README | 24 - iocBoot/iocSim/motor.substitutions | 14 - iocBoot/iocSim/st.cmd.Vx | 27 - iocBoot/iocSim/st.cmd.unix | 24 - iocBoot/iocSim/st.cmd.win32 | 14 - iocBoot/iocSim/start_medm | 1 - iocBoot/iocWithAsyn/motor.substitutions.sim | 9 - iocBoot/iocWithAsyn/st.cmd.sim | 19 - iocsh/EXAMPLE_motorSim.substitutions | 7 - iocsh/motorSim.iocsh | 34 - modules/Makefile | 1 + modules/motorMotorSim | 1 + motorApp/Makefile | 8 - motorApp/MotorSimSrc/Makefile | 63 - motorApp/MotorSimSrc/devMotorSim.c | 331 ----- motorApp/MotorSimSrc/drvMotorSim.c | 822 ------------ motorApp/MotorSimSrc/drvMotorSim.h | 13 - motorApp/MotorSimSrc/motorSimDriver.cpp | 553 -------- motorApp/MotorSimSrc/motorSimDriver.h | 84 -- motorApp/MotorSimSrc/motorSimMain.cpp | 23 - motorApp/MotorSimSrc/motorSimRegister.cc | 40 - motorApp/MotorSimSrc/motorSimSupport.dbd | 4 - motorApp/MotorSimSrc/motorSimTest.db | 60 - motorApp/MotorSimSrc/motorSimTest.src | 38 - motorApp/MotorSimSrc/route.c | 1320 ------------------- motorApp/MotorSimSrc/route.h | 69 - 28 files changed, 5 insertions(+), 3610 deletions(-) delete mode 100755 iocBoot/iocSim/Makefile delete mode 100644 iocBoot/iocSim/README delete mode 100644 iocBoot/iocSim/motor.substitutions delete mode 100644 iocBoot/iocSim/st.cmd.Vx delete mode 100644 iocBoot/iocSim/st.cmd.unix delete mode 100755 iocBoot/iocSim/st.cmd.win32 delete mode 100755 iocBoot/iocSim/start_medm delete mode 100644 iocBoot/iocWithAsyn/motor.substitutions.sim delete mode 100644 iocBoot/iocWithAsyn/st.cmd.sim delete mode 100644 iocsh/EXAMPLE_motorSim.substitutions delete mode 100644 iocsh/motorSim.iocsh create mode 160000 modules/motorMotorSim delete mode 100644 motorApp/MotorSimSrc/Makefile delete mode 100644 motorApp/MotorSimSrc/devMotorSim.c delete mode 100644 motorApp/MotorSimSrc/drvMotorSim.c delete mode 100644 motorApp/MotorSimSrc/drvMotorSim.h delete mode 100644 motorApp/MotorSimSrc/motorSimDriver.cpp delete mode 100644 motorApp/MotorSimSrc/motorSimDriver.h delete mode 100644 motorApp/MotorSimSrc/motorSimMain.cpp delete mode 100644 motorApp/MotorSimSrc/motorSimRegister.cc delete mode 100644 motorApp/MotorSimSrc/motorSimSupport.dbd delete mode 100644 motorApp/MotorSimSrc/motorSimTest.db delete mode 100644 motorApp/MotorSimSrc/motorSimTest.src delete mode 100644 motorApp/MotorSimSrc/route.c delete mode 100644 motorApp/MotorSimSrc/route.h diff --git a/.gitmodules b/.gitmodules index aea2420e..6c15eb1d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -88,3 +88,6 @@ [submodule "modules/motorPC"] path = modules/motorPC url = https://github.com/epics-motor/motorPC.git +[submodule "modules/motorMotorSim"] + path = modules/motorMotorSim + url = https://github.com/epics-motor/motorMotorSim.git diff --git a/iocBoot/iocSim/Makefile b/iocBoot/iocSim/Makefile deleted file mode 100755 index e26d2467..00000000 --- a/iocBoot/iocSim/Makefile +++ /dev/null @@ -1,9 +0,0 @@ -TOP = ../.. -include $(TOP)/configure/CONFIG -ARCH = linux-x86 -#ARCH = linux-x86_64 -#ARCH = vxWorks-ppc604 -TARGETS = cdCommands -TARGETS += envPaths -include $(TOP)/configure/RULES.ioc - diff --git a/iocBoot/iocSim/README b/iocBoot/iocSim/README deleted file mode 100644 index f7ddad1a..00000000 --- a/iocBoot/iocSim/README +++ /dev/null @@ -1,24 +0,0 @@ -To build any examples; - -- in /configure/RELEASE: EPICS_BASE and MOTOR must be defined. - -- in /Makefile: the following three lines must be uncommented; - #!DIRS += motorExApp iocBoot - #!motorExApp_DEPEND_DIRS = motorApp - #!iocBoot_DEPEND_DIRS = motorExApp - -To build this simulation example; - -- in /configure/RELEASE: ASYN must be defined. - -- in /iocBoot/iocSim/Makefile set the target architecture - -Finally, cd ; gnumake clean uninstall; gnumake - -To run this simulation example on a Unix OS; -- Set the EPICS_HOST_ARCH environment variable correctly. -- Edit the st.cmd.unix file for either sun or linux. -- Start the ioc from this directory by executing the following command. - -../../bin/${EPICS_HOST_ARCH}/WithAsyn st.cmd.unix - diff --git a/iocBoot/iocSim/motor.substitutions b/iocBoot/iocSim/motor.substitutions deleted file mode 100644 index 4cc11239..00000000 --- a/iocBoot/iocSim/motor.substitutions +++ /dev/null @@ -1,14 +0,0 @@ -file "../../db/basic_asyn_motor.db" -{ -pattern -{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT} -{IOC:, 1, "m$(N)", "asynMotor", motorSim1, 0, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 2, "m$(N)", "asynMotor", motorSim1, 1, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 3, "m$(N)", "asynMotor", motorSim1, 2, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 4, "m$(N)", "asynMotor", motorSim1, 3, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} - -{IOC:, 5, "m$(N)", "asynMotor", motorSim2, 0, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 6, "m$(N)", "asynMotor", motorSim2, 1, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 7, "m$(N)", "asynMotor", motorSim2, 2, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 8, "m$(N)", "asynMotor", motorSim2, 3, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -} diff --git a/iocBoot/iocSim/st.cmd.Vx b/iocBoot/iocSim/st.cmd.Vx deleted file mode 100644 index 99b539a8..00000000 --- a/iocBoot/iocSim/st.cmd.Vx +++ /dev/null @@ -1,27 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. - -# The following must be added for many board support packages -#!cd "... IOC st.cmd complete directory path ... " - -< cdCommands -#!< ../nfsCommands - -cd topbin - -# If the VxWorks kernel was built using the project facility, the following must -# be added before any C++ code is loaded (see SPR #28980). -sysCplusEnable=1 - -ld(0,0,"WithAsynVx.munch") - -cd startup -dbLoadDatabase("$(TOP)/dbd/WithAsynVx.dbd") -WithAsynVx_registerRecordDeviceDriver(pdbbase) -dbLoadTemplate("motor.substitutions") - -# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) -motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) -# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) -drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) -iocInit diff --git a/iocBoot/iocSim/st.cmd.unix b/iocBoot/iocSim/st.cmd.unix deleted file mode 100644 index 80853538..00000000 --- a/iocBoot/iocSim/st.cmd.unix +++ /dev/null @@ -1,24 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. - -< envPaths - -dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") -WithAsyn_registerRecordDeviceDriver(pdbbase) -dbLoadTemplate("motor.substitutions") - -# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) -motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) -# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) -drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) - -motorSimCreateController("motorSim2", 8) -#asynSetTraceIOMask("motorSim2", 0, 4) -#asynSetTraceMask("motorSim2", 0, 255) - -# motorSimConfigAxis(port, axis, lowLimit, highLimit, home, start) -motorSimConfigAxis("motorSim2", 0, 20000, -20000, 500, 0) -motorSimConfigAxis("motorSim2", 1, 20000, -20000, 1500, 0) -motorSimConfigAxis("motorSim2", 2, 20000, -20000, 2500, 0) -motorSimConfigAxis("motorSim2", 3, 20000, -20000, 3000, 0) -iocInit diff --git a/iocBoot/iocSim/st.cmd.win32 b/iocBoot/iocSim/st.cmd.win32 deleted file mode 100755 index 7f861f0c..00000000 --- a/iocBoot/iocSim/st.cmd.win32 +++ /dev/null @@ -1,14 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. - -< envPaths - -dbLoadDatabase("$(TOP)/dbd/WithMPFWin32.dbd") -WithMPFWin32_registerRecordDeviceDriver(pdbbase) -dbLoadTemplate("motor.substitutions") - -# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) -motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) -# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) -drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) -iocInit diff --git a/iocBoot/iocSim/start_medm b/iocBoot/iocSim/start_medm deleted file mode 100755 index 9cd0cba7..00000000 --- a/iocBoot/iocSim/start_medm +++ /dev/null @@ -1 +0,0 @@ -medm -x -macro "P=IOC:, M1=m1, M2=m2, M3=m3, M4=m4, M5=m5, M6=m6, M7=m7, M8=m8" motor8x.adl & diff --git a/iocBoot/iocWithAsyn/motor.substitutions.sim b/iocBoot/iocWithAsyn/motor.substitutions.sim deleted file mode 100644 index 98e78713..00000000 --- a/iocBoot/iocWithAsyn/motor.substitutions.sim +++ /dev/null @@ -1,9 +0,0 @@ -file "../../db/basic_asyn_motor.db" -{ -pattern -{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT} -{IOC:, 1, "m$(N)", "asynMotor", motorSim1, 0, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 2, "m$(N)", "asynMotor", motorSim1, 1, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 3, "m$(N)", "asynMotor", motorSim1, 2, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{IOC:, 4, "m$(N)", "asynMotor", motorSim1, 3, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -} diff --git a/iocBoot/iocWithAsyn/st.cmd.sim b/iocBoot/iocWithAsyn/st.cmd.sim deleted file mode 100644 index 91ff22c7..00000000 --- a/iocBoot/iocWithAsyn/st.cmd.sim +++ /dev/null @@ -1,19 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. - -< envPaths - -dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") -WithAsyn_registerRecordDeviceDriver(pdbbase) -dbLoadTemplate("motor.substitutions.sim") - -motorSimCreateController("motorSim1", 4) -#asynSetTraceIOMask("motorSim1", 0, 4) -#asynSetTraceMask("motorSim1", 0, 255) - -# motorSimConfigAxis(port, axis, lowLimit, highLimit, home, start) -motorSimConfigAxis("motorSim1", 0, 20000, -20000, 500, 0) -motorSimConfigAxis("motorSim1", 1, 20000, -20000, 1500, 0) -motorSimConfigAxis("motorSim1", 2, 20000, -20000, 2500, 0) -motorSimConfigAxis("motorSim1", 3, 20000, -20000, 3000, 0) -iocInit diff --git a/iocsh/EXAMPLE_motorSim.substitutions b/iocsh/EXAMPLE_motorSim.substitutions deleted file mode 100644 index 4ce68748..00000000 --- a/iocsh/EXAMPLE_motorSim.substitutions +++ /dev/null @@ -1,7 +0,0 @@ -file "$(MOTOR)/db/asyn_motor.db" -{ -pattern -{N, M, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, INIT} -{1, "m$(N)", 0, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, ""} -#{2, "m$(N)", 1, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, ""} -} diff --git a/iocsh/motorSim.iocsh b/iocsh/motorSim.iocsh deleted file mode 100644 index 80eb0adb..00000000 --- a/iocsh/motorSim.iocsh +++ /dev/null @@ -1,34 +0,0 @@ -# ### motorSim.iocsh ### - -#- ################################################### -#- PREFIX - IOC Prefix -#- INSTANCE - Instance name, used to create the low-level driver drvet name -#- Combined with the controller number to create the asyn port name -#- -#- SUB - Optional: Subsitutions file (asyn_motor.db), Macros P, DTYP, PORT, -#- DHLM, DLLM, and INIT will be predefined. -#- Default: $(MOTOR)/iocsh/EXAMPLE_motorSim.substitutions -#- -#- CONTROLLER - Optional: Which controller is being configured -#- Default: 0 -#- -#- NUM_AXES - Optional: Number of axes on this controller -#- Default: 1 -#- -#- LOW_LIM - Optional: Low Limit -#- Default: -32000 -#- -#- HIGH_LIM - Optional: High Limit -#- Default: 32000 -#- -#- HOME_POS - Optional: Home position -#- Default: 0 -#- ################################################### - -# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) -motorSimCreate($(CONTROLLER=0), 0, $(LOW_LIM=-32000), $(HIGH_LIM=32000), $(HOME_POS=0), 1, $(NUM_AXES=1)) - -# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) -drvAsynMotorConfigure("$(INSTANCE)$(CONTROLLER=0)", "$(INSTANCE)", $(CONTROLLER=0), $(NUM_AXES=1)) - -dbLoadTemplate("$(SUB=$(MOTOR)/iocsh/EXAMPLE_motorSim.substitutions)", "P=$(PREFIX), DTYP='asynMotor', PORT=$(INSTANCE)$(CONTROLLER=0), DHLM=$(HIGH_LIM=32000), DLLM=$(LOW_LIM=-32000)") diff --git a/modules/Makefile b/modules/Makefile index fcdf3a7f..23ddfbf8 100644 --- a/modules/Makefile +++ b/modules/Makefile @@ -22,6 +22,7 @@ SUBMODULES += motorMclennan SUBMODULES += motorMicos SUBMODULES += motorMicroMo SUBMODULES += motorMicronix +SUBMODULES += motorMotorSim SUBMODULES += motorNewFocus SUBMODULES += motorNPoint SUBMODULES += motorOmsAsyn diff --git a/modules/motorMotorSim b/modules/motorMotorSim new file mode 160000 index 00000000..557d6021 --- /dev/null +++ b/modules/motorMotorSim @@ -0,0 +1 @@ +Subproject commit 557d60217aec7ade6e71b940daf9b2c61005062e diff --git a/motorApp/Makefile b/motorApp/Makefile index 988b046b..7f6fe141 100644 --- a/motorApp/Makefile +++ b/motorApp/Makefile @@ -12,14 +12,6 @@ DIRS += MotorSrc DIRS += SoftMotorSrc SoftMotorSrc_DEPEND_DIRS = MotorSrc -# All the following modules require ASYN. -ifdef ASYN - -DIRS += MotorSimSrc -MotorSimSrc_DEPEND_DIRS = MotorSrc - -endif - # Install the edl files #DIRS += opi diff --git a/motorApp/MotorSimSrc/Makefile b/motorApp/MotorSimSrc/Makefile deleted file mode 100644 index cfde6f15..00000000 --- a/motorApp/MotorSimSrc/Makefile +++ /dev/null @@ -1,63 +0,0 @@ -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 += devMotorSim.c -motorSimSupport_SRCS += drvMotorSim.c -motorSimSupport_SRCS += motorSimDriver.cpp -motorSimSupport_SRCS += motorSimRegister.cc - -motorSimSupport_LIBS += motor -motorSimSupport_LIBS += asyn -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 += asyn.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 deleted file mode 100644 index f8889e3c..00000000 --- a/motorApp/MotorSimSrc/devMotorSim.c +++ /dev/null @@ -1,331 +0,0 @@ -/* devXxxSoft.c */ -/* Example device support module */ - -#include -#include -#include -#include - -/* The following is needed to compile against Base R3.16.1 without a warning */ -#define USE_TYPED_RSET - -#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 -{ - epicsUInt32 status; /**< bit mask of errors and other binary information. The bit positions are in motor.h */ - epicsInt32 position; /**< Current motor position in motor steps (if not servoing) or demand position (if servoing) */ - epicsInt32 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( (struct dbCommon *)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 deleted file mode 100644 index d28abe7f..00000000 --- a/motorApp/MotorSimSrc/drvMotorSim.c +++ /dev/null @@ -1,822 +0,0 @@ -/* -FILENAME... drvMotorSim.c -USAGE... Simulated Motor Support. - -*/ - -/* - * - * - * Modification Log: - * ----------------- - * 2006-05-06 npr Added prolog - * 2009-02-11 rls lock/unlock motorAxisSetDouble(). - * 2009-06-18 rls - Matthew Pearson's fix for record seeing motorAxisDone True - * on 1st status update after a move; set motorAxisDone False - * in motorAxisDrvSET_t functions that start motion - * (motorAxisHome, motorAxisMove, motorAxisVelocityMove) and - * force a status update with a call to callCallback(). - * - Matthew Pearson added deferred move support. - * 2010-10-05 rls - MP's fix for deferred moves broken in drvMotorSim. - * 2012-10-09 rls - Added motorAxisforceCallback to support motor record - * GET_INFO commands. - * - */ - -#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 = - { - 14, - 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 */ - 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 */ - motorAxisforceCallback, /**< Pointer to function to request a poller status update */ - motorAxisProfileMove, /**< Pointer to function to execute a profile move */ - motorAxisTriggerProfile /**< Pointer to function to trigger a profile move */ - }; - -epicsExportAddress(drvet, motorSim); - -typedef enum { none, positionMove, velocityMove, homeReverseMove, homeForwardsMove } moveType; - -/* typedef struct motorAxis * AXIS_ID; */ - -typedef struct drvSim * DRVSIM_ID; -typedef struct drvSim -{ - AXIS_HDL pFirst; - epicsThreadId motorThread; - motorAxisLogFunc print; - void * logParam; - epicsTimeStamp now; - int movesDeferred; - int nAxes; -} motorSim_t; - -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; - motorAxisLogFunc print; - void * logParam; - epicsTimeStamp tLast; - epicsMutexId axisMutex; - double deferred_position; - int deferred_move; - int deferred_relative; - DRVSIM_ID pDrv; -} motorAxis; - - - -static int motorSimLogMsg( void * param, const motorAxisLogMask_t logMask, const char *pFormat, ...); -#define TRACE_FLOW motorAxisTraceFlow -#define TRACE_ERROR motorAxisTraceError - -static motorSim_t drv={ NULL, NULL, motorSimLogMsg, NULL, { 0, 0 } }; - -#define MAX(a,b) ((a)>(b)? (a): (b)) -#define MIN(a,b) ((a)<(b)? (a): (b)) -#define DELTA 0.1 - -/*Deferred moves functions.*/ -static int processDeferredMoves(const motorSim_t * pDrv); - -static void motorProcTask(motorSim_t *); - -static void motorAxisReportAxis( AXIS_HDL pAxis, int level ) -{ - printf( "Found driver for motorSim card %d, axis %d, mutex %p\n", pAxis->card, pAxis->axis, pAxis->axisMutex ); - - if (level > 0) - { - double lowSoftLimit=0.0; - double hiSoftLimit=0.0; - - printf( "Current position = %f, velocity = %f at current time: %f\n", - pAxis->nextpoint.axis[0].p, - pAxis->nextpoint.axis[0].v, - pAxis->nextpoint.T ); - printf( "Destination posn = %f, velocity = %f at desination time: %f\n", - pAxis->endpoint.axis[0].p, - pAxis->endpoint.axis[0].v, - pAxis->endpoint.T ); - - printf( "Hard limits: %f, %f\n", pAxis->lowHardLimit, pAxis->hiHardLimit ); - motorParam->getDouble( pAxis->params, motorAxisHighLimit, &hiSoftLimit ); - motorParam->getDouble( pAxis->params, motorAxisLowLimit, &lowSoftLimit ); - printf( "Soft limits: %f, %f\n", lowSoftLimit, hiSoftLimit ); - - if (pAxis->homing) printf( "Currently homing axis\n" ); - - motorParam->dump( 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( AXIS_HDL pAxis, motorAxisLogFunc logFunc, void * param ) -{ - if (pAxis == NULL) - { - if (logFunc == NULL) - { - drv.print=motorSimLogMsg; - drv.logParam = NULL; - } - else - { - drv.print=logFunc; - drv.logParam = param; - } - } - else - { - if (logFunc == NULL) - { - pAxis->print=motorSimLogMsg; - pAxis->logParam = NULL; - } - else - { - pAxis->print=logFunc; - pAxis->logParam = param; - } - } - 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 - { - switch (function) { - case motorAxisDeferMoves: - *value = pAxis->pDrv->movesDeferred; - return MOTOR_AXIS_OK; - default: - return motorParam->getInteger( pAxis->params, (paramIndex) function, value ); - } - } -} - -static int motorAxisGetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double * value ) -{ - if (pAxis == NULL) return MOTOR_AXIS_ERROR; - else - { - switch (function) { - case motorAxisDeferMoves: - *value = (double)pAxis->pDrv->movesDeferred; - return MOTOR_AXIS_OK; - default: - return motorParam->getDouble( pAxis->params, (paramIndex) function, value ); - } - } -} - -static int motorAxisSetCallback( AXIS_HDL pAxis, motorAxisCallbackFunc callback, void * param ) -{ - if (pAxis == NULL) return MOTOR_AXIS_ERROR; - else - { - return motorParam->setCallback( pAxis->params, callback, param ); - } -} - -static int processDeferredMoves(const motorSim_t * pDrv) -{ - int status = MOTOR_AXIS_ERROR; - double position = 0.0; - AXIS_HDL pAxis = NULL; - - for ( pAxis = pDrv->pFirst; pAxis != NULL; pAxis = pAxis->pNext ) - { - if (pAxis->deferred_move) { - - position = pAxis->deferred_position; - - /* 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) - { - pAxis->endpoint.axis[0].p = position - pAxis->enc_offset; - pAxis->endpoint.axis[0].v = 0.0; - - motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); - - pAxis->deferred_move = 0; - epicsMutexUnlock( pAxis->axisMutex ); - } - - } - } - - return status; -} - - -static int motorAxisSetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double value ) -{ - int status = MOTOR_AXIS_OK; - - if (pAxis == NULL) return MOTOR_AXIS_ERROR; - else - { - epicsMutexLock(pAxis->axisMutex); - switch (function) - { - case motorAxisPosition: - { - pAxis->enc_offset = value - pAxis->nextpoint.axis[0].p; - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to position %f", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisResolution: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d resolution to %f", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisEncoderRatio: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to enc. ratio to %f", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisLowLimit: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d low limit to %f", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisHighLimit: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d high limit to %f", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisPGain: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d pgain to %f", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisIGain: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d igain to %f", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisDGain: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d dgain to %f", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisClosedLoop: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d closed loop to %s", pAxis->card, pAxis->axis, (value!=0?"ON":"OFF") ); - break; - } - case motorAxisDeferMoves: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, - "%sing Deferred Move flag on PMAC card %d\n", - value != 0.0?"Sett":"Clear",pAxis->card); - if (value == 0.0 && pAxis->pDrv->movesDeferred != 0) { - processDeferredMoves(pAxis->pDrv); - } - pAxis->pDrv->movesDeferred = (int)value; - break; - } - default: - status = MOTOR_AXIS_ERROR; - break; - } - if (status == MOTOR_AXIS_OK ) - { - motorParam->setDouble(pAxis->params, function, value); - motorParam->callCallback(pAxis->params); - } - epicsMutexUnlock(pAxis->axisMutex); - } - 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 - { - if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) - { - switch (function) - { - case motorAxisPosition: - { - pAxis->enc_offset = (double) value - pAxis->nextpoint.axis[0].p; - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to position %d", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisLowLimit: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d low limit to %d", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisHighLimit: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d high limit to %d", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisClosedLoop: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d closed loop to %s", pAxis->card, pAxis->axis, (value?"ON":"OFF") ); - break; - } - case motorAxisDeferMoves: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, - "%sing Deferred Move flag on PMAC card %d\n", - value != 0.0?"Sett":"Clear",pAxis->card); - if (value == 0.0 && pAxis->pDrv->movesDeferred != 0) - { - processDeferredMoves(pAxis->pDrv); - } - pAxis->pDrv->movesDeferred = value; - break; - } - default: - status = MOTOR_AXIS_ERROR; - break; - } - - if (status != MOTOR_AXIS_ERROR ) - { - status = motorParam->setInteger( pAxis->params, function, value ); - motorParam->callCallback(pAxis->params); - } - epicsMutexUnlock(pAxis->axisMutex); - } - 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; - - if (pAxis->pDrv->movesDeferred == 0) { /*Normal move.*/ - pAxis->endpoint.axis[0].p = position - pAxis->enc_offset; - pAxis->endpoint.axis[0].v = 0.0; - } else { /*Deferred moves.*/ - pAxis->deferred_position = position; - pAxis->deferred_move = 1; - pAxis->deferred_relative = relative; - } - 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 ); - - motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); - motorParam->callCallback( pAxis->params ); - epicsMutexUnlock( pAxis->axisMutex ); - - pAxis->print( pAxis->logParam, TRACE_FLOW, "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 - { - double time; - - routeGetParams( pAxis->route, &pars ); - if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration); - routeSetParams( pAxis->route, &pars ); - - time = fabs( deltaV / pars.axis[0].Amax ); - - pAxis->endpoint.axis[0].v = velocity; - pAxis->endpoint.axis[0].p = ( pAxis->nextpoint.axis[0].p + - time * ( pAxis->nextpoint.axis[0].v + 0.5 * deltaV )); - 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 - { - if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) { - status = motorAxisVelocity( pAxis, (forwards? max_velocity: -max_velocity), acceleration ); - pAxis->homing = 1; - motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); - motorParam->callCallback( pAxis->params ); - epicsMutexUnlock( pAxis->axisMutex ); - pAxis->print( pAxis->logParam, TRACE_FLOW, "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 ); - motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); - motorParam->callCallback( pAxis->params ); - epicsMutexUnlock( pAxis->axisMutex ); - pAxis->print( pAxis->logParam, TRACE_FLOW, "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 - { - if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) { - motorAxisVelocity( pAxis, 0.0, acceleration ); - pAxis->deferred_move = 0; - epicsMutexUnlock( pAxis->axisMutex ); - - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to stop with accel=%f", - pAxis->card, pAxis->axis, acceleration ); - } - - } - return MOTOR_AXIS_OK; -} - -static int motorAxisforceCallback(AXIS_HDL pAxis) -{ - if (pAxis == NULL) - return(MOTOR_AXIS_ERROR); - - /* Force a status update. */ - motorParam->forceCallback(pAxis->params); - motorProcTask(&drv); - 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; - int done = 0; - - 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; - } - } - - if (pAxis->nextpoint.axis[0].v == 0) { - if (!pAxis->deferred_move) { - done = 1; - } - } else { - done = 0; - } - - motorParam->setDouble( pAxis->params, motorAxisPosition, (pAxis->nextpoint.axis[0].p+pAxis->enc_offset) ); - motorParam->setDouble( pAxis->params, motorAxisEncoderPosn, (pAxis->nextpoint.axis[0].p+pAxis->enc_offset) ); - motorParam->setInteger( pAxis->params, motorAxisDirection, (pAxis->nextpoint.axis[0].v > 0) ); - motorParam->setInteger( pAxis->params, motorAxisDone, done ); - motorParam->setInteger( pAxis->params, motorAxisHighHardLimit, (pAxis->nextpoint.axis[0].p >= pAxis->hiHardLimit) ); - motorParam->setInteger( pAxis->params, motorAxisHomeSignal, (pAxis->nextpoint.axis[0].p == pAxis->home) ); - motorParam->setInteger( pAxis->params, motorAxisMoving, !done ); - motorParam->setInteger( pAxis->params, motorAxisLowHardLimit, (pAxis->nextpoint.axis[0].p <= pAxis->lowHardLimit) ); -} - - -static void motorProcTask( motorSim_t *pDrv) -{ - epicsTimeStamp now; - double delta; - AXIS_HDL pAxis; - - /* 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 ); - motorParam->callCallback( pAxis->params ); - epicsMutexUnlock( pAxis->axisMutex ); - } - } - } -} - -static void motorSimTask( motorSim_t * pDrv ) -{ - while ( 1 ) - { - motorProcTask(pDrv); - epicsThreadSleep( DELTA ); - } -} - -static int motorSimCreateAxis( motorSim_t * pDrv, int card, int axis, double lowLimit, double hiLimit, double home, double start ) -{ - AXIS_HDL pAxis; - AXIS_HDL * ppLast = &(pDrv->pFirst); - start=0; - - 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; - - pAxis->pDrv = pDrv; - - 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 = start; - pAxis->endpoint.axis[0].v = 0; - pAxis->nextpoint.axis[0].p = start; - - if ((pAxis->route = routeNew( &(pAxis->endpoint), &pars )) != NULL && - (pAxis->params = motorParam->create( 0, MOTOR_AXIS_NUM_PARAMS )) != NULL && - (pAxis->axisMutex = epicsMutexCreate( )) != NULL ) - { - pAxis->card = card; - pAxis->axis = axis; - pAxis->hiHardLimit = hiLimit; - pAxis->lowHardLimit = lowLimit; - pAxis->home = home; - pAxis->print = motorSimLogMsg; - pAxis->logParam = NULL; - motorParam->setDouble(pAxis->params, motorAxisPosition, start); - *ppLast = pAxis; - pAxis->print( pAxis->logParam, TRACE_FLOW, "Created motor for card %d, signal %d OK", card, axis ); - } - else - { - if (pAxis->route != NULL) routeDelete( pAxis->route ); - if (pAxis->params != NULL) motorParam->destroy( pAxis->params ); - if (pAxis->axisMutex != NULL) epicsMutexDestroy( pAxis->axisMutex ); - free ( pAxis ); - pAxis = NULL; - } - } - else - { - free ( pAxis ); - pAxis = NULL; - } - } - else - { - pAxis->print( pAxis->logParam, TRACE_ERROR, "Motor for card %d, signal %d already exists", card, axis ); - return MOTOR_AXIS_ERROR; - } - - if (pAxis == NULL) - { - pAxis->print( pAxis->logParam, TRACE_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, int lowLimit, int hiLimit, int home, int nCards, int nAxes, int startPosn ) -{ - int i; - int j; - - if (nCards < 1) nCards = 1; - if (nAxes < 1 ) nAxes = 1; - - drv.nAxes = nAxes; - - drv.print( drv.logParam, TRACE_FLOW, - "Creating motor simulator: card: %d, axis: %d, hi: %d, low %d, home: %d, 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) - { - drv.print( drv.logParam, TRACE_ERROR, "Cannot start motor simulation thread" ); - return; - } - } - - for ( i = card; i < card+nCards; i++ ) - { - for (j = axis; j < axis+nAxes; j++ ) - { - motorSimCreateAxis( &drv, i, j, (double) lowLimit, (double) hiLimit, (double) home, (double) startPosn ); - } - } -} - -static int motorSimLogMsg( void * param, const motorAxisLogMask_t mask, const char *pFormat, ...) -{ - - va_list pvar; - int nchar; - - va_start(pvar, pFormat); - nchar = vfprintf(stdout,pFormat,pvar); - va_end (pvar); - fprintf(stdout,"\n"); - return(nchar); -} diff --git a/motorApp/MotorSimSrc/drvMotorSim.h b/motorApp/MotorSimSrc/drvMotorSim.h deleted file mode 100644 index e1b26ed6..00000000 --- a/motorApp/MotorSimSrc/drvMotorSim.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef DRV_MOTOR_SIM_H -#define DRV_MOTOR_SIM_H - -#ifdef __cplusplus -extern "C" { -#endif - -void motorSimCreate( int card, int axis, int hiLimit, int lowLimit, int home, int nCards, int nAxes, int startPosn ); - -#ifdef __cplusplus -} -#endif -#endif diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp deleted file mode 100644 index bc3e12a4..00000000 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ /dev/null @@ -1,553 +0,0 @@ -/* -FILENAME... motorSimController.cpp -USAGE... Simulated Motor Support. - -Based on drvMotorSim.c - -Mark Rivers -December 13, 2009 - -*/ - - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "asynMotorController.h" -#include "asynMotorAxis.h" - -#include -#include "motorSimDriver.h" - -#define DEFAULT_LOW_LIMIT -10000 -#define DEFAULT_HI_LIMIT 10000 -#define DEFAULT_HOME 0 -#define DEFAULT_START 0 - -static const char *driverName = "motorSimDriver"; - -static void motorSimTaskC(void *drvPvt); - -typedef struct motorSimControllerNode { - ELLNODE node; - const char *portName; - motorSimController *pController; -} motorSimControllerNode; - -static ELLLIST motorSimControllerList; -static int motorSimControllerListInitialized = 0; - -motorSimAxis::motorSimAxis(motorSimController *pController, int axis, double lowHardLimit, double hiHardLimit, double home, double start ) - : asynMotorAxis(pController, axis), - pC_(pController), - lowHardLimit_(lowHardLimit), hiHardLimit_(hiHardLimit), home_(home) -{ - 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; - - endpoint_.T = 0; - endpoint_.axis[0].p = start; - endpoint_.axis[0].v = 0; - nextpoint_.T = 0; - nextpoint_.axis[0].p = start; - route_ = routeNew( &(this->endpoint_), &pars ); - deferred_move_ = 0; -} - - -motorSimController::motorSimController(const char *portName, int numAxes, int priority, int stackSize) - : asynMotorController(portName, numAxes, NUM_SIM_CONTROLLER_PARAMS, - asynInt32Mask | asynFloat64Mask, - asynInt32Mask | asynFloat64Mask, - ASYN_CANBLOCK | ASYN_MULTIDEVICE, - 1, // autoconnect - priority, stackSize) -{ - int axis; - motorSimControllerNode *pNode; - - if (!motorSimControllerListInitialized) { - motorSimControllerListInitialized = 1; - ellInit(&motorSimControllerList); - } - - // We should make sure this portName is not already in the list */ - pNode = (motorSimControllerNode*) calloc(1, sizeof(motorSimControllerNode)); - pNode->portName = epicsStrDup(portName); - pNode->pController = this; - ellAdd(&motorSimControllerList, (ELLNODE *)pNode); - - if (numAxes < 1 ) numAxes = 1; - numAxes_ = numAxes; - this->movesDeferred_ = 0; - for (axis=0; axismotorPosition_, DEFAULT_START); - } - - this->motorThread_ = epicsThreadCreate("motorSimThread", - epicsThreadPriorityLow, - epicsThreadGetStackSize(epicsThreadStackMedium), - (EPICSTHREADFUNC) motorSimTaskC, (void *) this); -} - -void motorSimController::report(FILE *fp, int level) -{ - int axis; - motorSimAxis *pAxis; - - fprintf(fp, "Simulation motor driver %s, numAxes=%d\n", - this->portName, numAxes_); - - for (axis=0; axisaxisNo_); - - if (level > 0) - { - double lowSoftLimit=0.0; - double hiSoftLimit=0.0; - - fprintf(fp, " Current position = %f, velocity = %f at current time: %f\n", - pAxis->nextpoint_.axis[0].p, - pAxis->nextpoint_.axis[0].v, - pAxis->nextpoint_.T); - fprintf(fp, " Destination posn = %f, velocity = %f at desination time: %f\n", - pAxis->endpoint_.axis[0].p, - pAxis->endpoint_.axis[0].v, - pAxis->endpoint_.T); - - fprintf(fp, " Hard limits: %f, %f\n", pAxis->lowHardLimit_, pAxis->hiHardLimit_); - fprintf(fp, " Home: %f\n", pAxis->home_); - fprintf(fp, " Enc. offset: %f\n", pAxis->enc_offset_); - getDoubleParam(pAxis->axisNo_, motorHighLimit_, &hiSoftLimit); - getDoubleParam(pAxis->axisNo_, motorLowLimit_, &lowSoftLimit); - fprintf(fp, " Soft limits: %f, %f\n", lowSoftLimit, hiSoftLimit ); - - if (pAxis->homing_) fprintf(fp, " Currently homing axis\n" ); - } - } - - // Call the base class method - asynMotorController::report(fp, level); -} - -asynStatus motorSimController::processDeferredMoves() -{ - asynStatus status = asynError; - double position = 0.0; - int axis; - motorSimAxis *pAxis; - - for (axis=0; axisdeferred_move_) { - position = pAxis->deferred_position_; - /* 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 asynError; - pAxis->endpoint_.axis[0].p = position - pAxis->enc_offset_; - pAxis->endpoint_.axis[0].v = 0.0; - setIntegerParam(axis, motorStatusDone_, 0); - pAxis->deferred_move_ = 0; - } - } - return status; -} - -asynStatus motorSimController::writeInt32(asynUser *pasynUser, epicsInt32 value) -{ - int function = pasynUser->reason; - asynStatus status = asynSuccess; - motorSimAxis *pAxis = this->getAxis(pasynUser); - static const char *functionName = "writeInt32"; - - - /* Set the parameter and readback in the parameter library. This may be overwritten when we read back the - * status at the end, but that's OK */ - pAxis->setIntegerParam(function, value); - - if (function == motorDeferMoves_) - { - asynPrint(pasynUser, ASYN_TRACE_FLOW, - "%s:%s: %sing Deferred Move flag on driver %s\n", - value != 0.0?"Sett":"Clear", - driverName, functionName, this->portName); - if (value == 0.0 && movesDeferred_ != 0) - { - processDeferredMoves(); - } - movesDeferred_ = value; - } else { - /* Call base class call its method (if we have our parameters check this here) */ - status = asynMotorController::writeInt32(pasynUser, value); - } - - /* Do callbacks so higher layers see any changes */ - pAxis->callParamCallbacks(); - if (status) - asynPrint(pasynUser, ASYN_TRACE_ERROR, - "%s:%s: error, status=%d function=%d, value=%d\n", - driverName, functionName, status, function, value); - else - asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, - "%s:%s: function=%d, value=%d\n", - driverName, functionName, function, value); - return status; -} - -motorSimAxis* motorSimController::getAxis(asynUser *pasynUser) -{ - return static_cast(asynMotorController::getAxis(pasynUser)); -} - -motorSimAxis* motorSimController::getAxis(int axisNo) -{ - return static_cast(asynMotorController::getAxis(axisNo)); -} - -asynStatus motorSimController::profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger ) -{ - return asynError; -} - -asynStatus motorSimController::triggerProfile(asynUser *pasynUser) -{ - return asynError; -} - -static void motorSimTaskC(void *drvPvt) -{ - motorSimController *pController = (motorSimController*)drvPvt; - pController->motorSimTask(); -} - - -#define DELTA 0.1 -void motorSimController::motorSimTask() -{ - epicsTimeStamp now; - double delta; - int axis; - motorSimAxis *pAxis; - - while ( 1 ) - { - /* Get a new timestamp */ - epicsTimeGetCurrent( &now ); - delta = epicsTimeDiffInSeconds( &now, &(prevTime_) ); - prevTime_ = 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 (axis=0; axislock(); - pAxis = getAxis(axis); - pAxis->process(delta ); - this->unlock(); - } - } - epicsThreadSleep( DELTA ); - } -} - -asynStatus motorSimAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) -{ - route_pars_t pars; - static const char *functionName = "move"; - - if (relative) position += endpoint_.axis[0].p + enc_offset_; - - /* Check to see if in hard limits */ - if ((nextpoint_.axis[0].p >= hiHardLimit_ && position > nextpoint_.axis[0].p) || - (nextpoint_.axis[0].p <= lowHardLimit_ && position < nextpoint_.axis[0].p) ) return asynError; - - if (pC_->movesDeferred_ == 0) { /*Normal move.*/ - endpoint_.axis[0].p = position - enc_offset_; - endpoint_.axis[0].v = 0.0; - } else { /*Deferred moves.*/ - deferred_position_ = position; - deferred_move_ = 1; - deferred_relative_ = relative; - } - routeGetParams(route_, &pars); - if (maxVelocity != 0) pars.axis[0].Vmax = fabs(maxVelocity); - if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration); - routeSetParams( route_, &pars ); - - setIntegerParam(pC_->motorStatusDone_, 0); - callParamCallbacks(); - - asynPrint(pasynUser_, ASYN_TRACE_FLOW, - "%s:%s: Set driver %s, axis %d move to %f, min vel=%f, maxVel=%f, accel=%f\n", - driverName, functionName, pC_->portName, axisNo_, position, minVelocity, maxVelocity, acceleration ); - return asynSuccess; -} - -asynStatus motorSimAxis::setVelocity(double velocity, double acceleration ) -{ - route_pars_t pars; - double deltaV = velocity - this->nextpoint_.axis[0].v; - double time; - - /* Check to see if in hard limits */ - if ((this->nextpoint_.axis[0].p > hiHardLimit_ && velocity > 0) || - (this->nextpoint_.axis[0].p < lowHardLimit_ && velocity < 0) ) return asynError; - - routeGetParams( this->route_, &pars ); - if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration); - routeSetParams( this->route_, &pars ); - - time = fabs( deltaV / pars.axis[0].Amax ); - - this->endpoint_.axis[0].v = velocity; - this->endpoint_.axis[0].p = (this->nextpoint_.axis[0].p + - time * ( this->nextpoint_.axis[0].v + 0.5 * deltaV)); - this->reroute_ = ROUTE_NEW_ROUTE; - return asynSuccess; -} - - -asynStatus motorSimAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards ) -{ - asynStatus status = asynError; - // static const char *functionName = "home"; - - status = setVelocity((forwards? maxVelocity: -maxVelocity), acceleration ); - homing_ = 1; - return status; -} - - -asynStatus motorSimAxis::moveVelocity(double minVelocity, double velocity, double acceleration ) -{ - asynStatus status = asynError; - // static const char *functionName = "moveVelocity"; - - status = setVelocity(velocity, acceleration ); - return status; -} - -asynStatus motorSimAxis::stop(double acceleration ) -{ - // static const char *functionName = "moveVelocityAxis"; - - setVelocity(0.0, acceleration ); - deferred_move_ = 0; - return asynSuccess; -} - -asynStatus motorSimAxis::setPosition(double position) -{ - enc_offset_ = position - nextpoint_.axis[0].p; - return asynSuccess; -} - -asynStatus motorSimAxis::config(int hiHardLimit, int lowHardLimit, int home, int start) -{ - hiHardLimit_ = hiHardLimit; - lowHardLimit_ = lowHardLimit; - home_ = home; - enc_offset_ = start; - return asynSuccess; -} - -asynStatus motorSimAxis::poll(bool *moving) -{ - return asynSuccess; -} - - -/** Process one iteration of an axis - - This routine takes a single axis and propogates its motion forward a given amount - of time. - - \param delta [in] Time in seconds to propogate motion forwards. - - \return Integer indicating 0 (asynSuccess) for success or non-zero for failure. -*/ - -void motorSimAxis::process(double delta ) -{ - double lastpos; - int done = 0; - double postMoveDelay = 0.0; - epicsTimeStamp nowTime; - double nowTimeSecs = 0.0; - - lastpos = nextpoint_.axis[0].p; - nextpoint_.T += delta; - routeFind( route_, reroute_, &endpoint_, &nextpoint_ ); - /* if (reroute_ == ROUTE_NEW_ROUTE) routePrint( route_, reroute_, &endpoint_, &nextpoint_, stdout ); */ - reroute_ = ROUTE_CALC_ROUTE; - - /* No, do a limits check */ - if (homing_ && - ((lastpos - home_) * (nextpoint_.axis[0].p - home_)) <= 0) - { - /* Homing and have crossed the home sensor - return to home */ - homing_ = 0; - reroute_ = ROUTE_NEW_ROUTE; - endpoint_.axis[0].p = home_; - endpoint_.axis[0].v = 0.0; - } - if ( nextpoint_.axis[0].p > hiHardLimit_ && nextpoint_.axis[0].v > 0 ) - { - if (homing_) setVelocity(-endpoint_.axis[0].v, 0.0 ); - else - { - reroute_ = ROUTE_NEW_ROUTE; - endpoint_.axis[0].p = hiHardLimit_; - endpoint_.axis[0].v = 0.0; - } - } - else if (nextpoint_.axis[0].p < lowHardLimit_ && nextpoint_.axis[0].v < 0) - { - if (homing_) setVelocity(-endpoint_.axis[0].v, 0.0 ); - else - { - reroute_ = ROUTE_NEW_ROUTE; - endpoint_.axis[0].p = lowHardLimit_; - endpoint_.axis[0].v = 0.0; - } - } - - if (nextpoint_.axis[0].v == 0) { - if (!deferred_move_) { - if (!delayedDone_) { - done = 1; - } - } - } else { - done = 0; - } - - //Post move delay - epicsTimeGetCurrent(&nowTime); - pC_->getDoubleParam(axisNo_, pC_->motorPostMoveDelay_, &postMoveDelay); - if ((lastDone_ == 0) && (done == 1)) { - if (postMoveDelay > 0) { - delayedDone_ = 1; - done = 0; - lastTimeSecs_ = nowTime.secPastEpoch + (nowTime.nsec / 1.e9); - } - } - if (delayedDone_ == 1) { - nowTimeSecs = nowTime.secPastEpoch + (nowTime.nsec / 1.e9); - if ((nowTimeSecs - lastTimeSecs_) >= postMoveDelay) { - done = 1; - delayedDone_ = 0; - } - } - - lastDone_ = done; - - setDoubleParam (pC_->motorPosition_, (nextpoint_.axis[0].p+enc_offset_)); - setDoubleParam (pC_->motorEncoderPosition_, (nextpoint_.axis[0].p+enc_offset_)); - setIntegerParam(pC_->motorStatusDirection_, (nextpoint_.axis[0].v > 0)); - setIntegerParam(pC_->motorStatusDone_, done); - setIntegerParam(pC_->motorStatusHighLimit_, (nextpoint_.axis[0].p >= hiHardLimit_)); - setIntegerParam(pC_->motorStatusHome_, (nextpoint_.axis[0].p == home_)); - setIntegerParam(pC_->motorStatusMoving_, !done); - setIntegerParam(pC_->motorStatusLowLimit_, (nextpoint_.axis[0].p <= lowHardLimit_)); - callParamCallbacks(); -} - -/** Configuration command, called directly or from iocsh */ -extern "C" int motorSimCreateController(const char *portName, int numAxes, int priority, int stackSize) -{ - new motorSimController(portName,numAxes, priority, stackSize); - return(asynSuccess); -} - -extern "C" int motorSimConfigAxis(const char *portName, int axis, int hiHardLimit, int lowHardLimit, int home, int start) -{ - motorSimControllerNode *pNode; - static const char *functionName = "motorSimConfigAxis"; - - // Find this controller - if (!motorSimControllerListInitialized) { - printf("%s:%s: ERROR, controller list not initialized\n", - driverName, functionName); - return(-1); - } - pNode = (motorSimControllerNode*)ellFirst(&motorSimControllerList); - while(pNode) { - if (strcmp(pNode->portName, portName) == 0) { - printf("%s:%s: configuring controller %s axis %d\n", - driverName, functionName, pNode->portName, axis); - pNode->pController->getAxis(axis)->config(hiHardLimit, lowHardLimit, home, start); - return(0); - } - pNode = (motorSimControllerNode*)ellNext((ELLNODE*)pNode); - } - printf("Controller not found\n"); - return(-1); -} - -/** Code for iocsh registration */ -static const iocshArg motorSimCreateControllerArg0 = {"Port name", iocshArgString}; -static const iocshArg motorSimCreateControllerArg1 = {"Number of axes", iocshArgInt}; -static const iocshArg motorSimCreateControllerArg2 = {"priority", iocshArgInt}; -static const iocshArg motorSimCreateControllerArg3 = {"stackSize", iocshArgInt}; -static const iocshArg * const motorSimCreateControllerArgs[] = {&motorSimCreateControllerArg0, - &motorSimCreateControllerArg1, - &motorSimCreateControllerArg2, - &motorSimCreateControllerArg3}; -static const iocshFuncDef motorSimCreateControllerDef = {"motorSimCreateController", 4, motorSimCreateControllerArgs}; -static void motorSimCreateContollerCallFunc(const iocshArgBuf *args) -{ - motorSimCreateController(args[0].sval, args[1].ival, args[2].ival, args[3].ival); -} - -static const iocshArg motorSimConfigAxisArg0 = { "Post name", iocshArgString}; -static const iocshArg motorSimConfigAxisArg1 = { "Axis #", iocshArgInt}; -static const iocshArg motorSimConfigAxisArg2 = { "High limit", iocshArgInt}; -static const iocshArg motorSimConfigAxisArg3 = { "Low limit", iocshArgInt}; -static const iocshArg motorSimConfigAxisArg4 = { "Home position", iocshArgInt}; -static const iocshArg motorSimConfigAxisArg5 = { "Start posn", iocshArgInt}; - -static const iocshArg *const motorSimConfigAxisArgs[] = { - &motorSimConfigAxisArg0, - &motorSimConfigAxisArg1, - &motorSimConfigAxisArg2, - &motorSimConfigAxisArg3, - &motorSimConfigAxisArg4, - &motorSimConfigAxisArg5 -}; -static const iocshFuncDef motorSimConfigAxisDef ={"motorSimConfigAxis",6,motorSimConfigAxisArgs}; - -static void motorSimConfigAxisCallFunc(const iocshArgBuf *args) -{ - motorSimConfigAxis(args[0].sval, args[1].ival, args[2].ival, args[3].ival, args[4].ival, args[5].ival); -} - -static void motorSimDriverRegister(void) -{ - - iocshRegister(&motorSimCreateControllerDef, motorSimCreateContollerCallFunc); - iocshRegister(&motorSimConfigAxisDef, motorSimConfigAxisCallFunc); -} - -extern "C" { -epicsExportRegistrar(motorSimDriverRegister); -} diff --git a/motorApp/MotorSimSrc/motorSimDriver.h b/motorApp/MotorSimSrc/motorSimDriver.h deleted file mode 100644 index 2c9e8146..00000000 --- a/motorApp/MotorSimSrc/motorSimDriver.h +++ /dev/null @@ -1,84 +0,0 @@ -/* -FILENAME... motorSimController.h -USAGE... Simulated Motor Support. - -Based on drvMotorSim.c - -Mark Rivers -March 28, 2010 - -*/ - - -#include -#include - -#include "asynMotorController.h" -#include "asynMotorAxis.h" -#include "route.h" - -#define NUM_SIM_CONTROLLER_PARAMS 0 - -class epicsShareClass motorSimAxis : public asynMotorAxis -{ -public: - - /* These are the pure virtual functions that must be implemented */ - motorSimAxis(class motorSimController *pController, int axis, double lowLimit, double hiLimit, double home, double start); - asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); - asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); - asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards); - asynStatus stop(double acceleration); - asynStatus poll(bool *moving); - asynStatus setPosition(double position); - - /* These are the methods that are new to this class */ - asynStatus config(int hiHardLimit, int lowHardLimit, int home, int start); - asynStatus setVelocity(double velocity, double acceleration); - void process(double delta ); - -private: - motorSimController *pC_; - ROUTE_ID route_; - route_reroute_t reroute_; - route_demand_t endpoint_; - route_demand_t nextpoint_; - double lowHardLimit_; - double hiHardLimit_; - double enc_offset_; - double home_; - int homing_; - epicsTimeStamp tLast_; - double deferred_position_; - int deferred_move_; - int deferred_relative_; - double lastTimeSecs_; - int delayedDone_; - int lastDone_; - -friend class motorSimController; -}; - -class epicsShareClass motorSimController : asynMotorController { -public: - - /* These are the fucntions we override from the base class */ - motorSimController(const char *portName, int numAxes, int priority, int stackSize); - asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); - void report(FILE *fp, int level); - motorSimAxis* getAxis(asynUser *pasynUser); - motorSimAxis* getAxis(int axisNo); - asynStatus profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger); - asynStatus triggerProfile(asynUser *pasynUser); - - /* These are the functions that are new to this class */ - void motorSimTask(); // Should be pivate, but called from non-member function - -private: - asynStatus processDeferredMoves(); - epicsThreadId motorThread_; - epicsTimeStamp prevTime_; - int movesDeferred_; - -friend class motorSimAxis; -}; diff --git a/motorApp/MotorSimSrc/motorSimMain.cpp b/motorApp/MotorSimSrc/motorSimMain.cpp deleted file mode 100644 index ab26c299..00000000 --- a/motorApp/MotorSimSrc/motorSimMain.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* 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 deleted file mode 100644 index b3c22383..00000000 --- a/motorApp/MotorSimSrc/motorSimRegister.cc +++ /dev/null @@ -1,40 +0,0 @@ -#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", iocshArgInt}; -static const iocshArg motorSimCreateArg3 = { "Low limit", iocshArgInt}; -static const iocshArg motorSimCreateArg4 = { "Home position", iocshArgInt}; -static const iocshArg motorSimCreateArg5 = { "Num cards", iocshArgInt}; -static const iocshArg motorSimCreateArg6 = { "Num signals", iocshArgInt}; -static const iocshArg motorSimCreateArg7 = { "Start posn", iocshArgInt}; - -static const iocshArg *const motorSimCreateArgs[] = { - &motorSimCreateArg0, - &motorSimCreateArg1, - &motorSimCreateArg2, - &motorSimCreateArg3, - &motorSimCreateArg4, - &motorSimCreateArg5, - &motorSimCreateArg6, - &motorSimCreateArg7, -}; -static const iocshFuncDef motorSimCreateDef ={"motorSimCreate",8,motorSimCreateArgs}; - -static void motorSimCreateCallFunc(const iocshArgBuf *args) -{ - motorSimCreate(args[0].ival, args[1].ival, args[2].ival, args[3].ival, args[4].ival, args[5].ival, args[6].ival, args[7].ival); -} - -void motorSimRegister(void) -{ - iocshRegister(&motorSimCreateDef, motorSimCreateCallFunc); -} -epicsExportRegistrar(motorSimRegister); - -} // extern "C" - diff --git a/motorApp/MotorSimSrc/motorSimSupport.dbd b/motorApp/MotorSimSrc/motorSimSupport.dbd deleted file mode 100644 index f6b21fd1..00000000 --- a/motorApp/MotorSimSrc/motorSimSupport.dbd +++ /dev/null @@ -1,4 +0,0 @@ -device(motor,VME_IO,devMotorSim,"Motor Simulation") -driver(motorSim) -registrar(motorSimRegister) -registrar(motorSimDriverRegister) diff --git a/motorApp/MotorSimSrc/motorSimTest.db b/motorApp/MotorSimSrc/motorSimTest.db deleted file mode 100644 index ca8a698c..00000000 --- a/motorApp/MotorSimSrc/motorSimTest.db +++ /dev/null @@ -1,60 +0,0 @@ -record(motor,"$(DEVICE):test0") -{ - 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") -} - -record(motor,"$(DEVICE):test1") -{ - 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 S1 @motorSim") - field(MRES,"0.001") - field(PREC,"5") - field(EGU,"mm") - field(DHLM,"30") - field(DLLM,"-30") -} - -record(motor,"$(DEVICE):test2") -{ - 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 S2 @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 deleted file mode 100644 index 244ae2b3..00000000 --- a/motorApp/MotorSimSrc/motorSimTest.src +++ /dev/null @@ -1,38 +0,0 @@ -#!$(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)/motorSim.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=motorSim") -#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, -22000, 42000, 10000, 1, 3, 5000 ) - -iocInit() - -## Start any sequence programs -#seq sncExample,"user=npr78Host" diff --git a/motorApp/MotorSimSrc/route.c b/motorApp/MotorSimSrc/route.c deleted file mode 100644 index c093f4a4..00000000 --- a/motorApp/MotorSimSrc/route.c +++ /dev/null @@ -1,1320 +0,0 @@ -#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 deleted file mode 100644 index 48b8d730..00000000 --- a/motorApp/MotorSimSrc/route.h +++ /dev/null @@ -1,69 +0,0 @@ -#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 */