From b240341bfdd9b6cdc0be3b3dde21b83972cb5eec Mon Sep 17 00:00:00 2001 From: smathis Date: Fri, 14 Feb 2025 15:12:33 +0100 Subject: [PATCH] Initial commit of the PSI version of motorMotorSim as a standalone driver. --- .gitattributes | 5 + .gitignore | 18 + GNUmakefile | 25 + Makefile | 44 + README.md | 7 + configure/CONFIG | 29 + configure/CONFIG_SITE | 43 + configure/Makefile | 12 + configure/RELEASE | 7 + configure/RULES | 6 + configure/RULES.ioc | 2 + configure/RULES_DIRS | 2 + configure/RULES_TOP | 3 + docs/RELEASE.md | 33 + iocs/Makefile | 25 + iocs/motorSimIOC/Makefile | 31 + iocs/motorSimIOC/configure/CONFIG | 29 + iocs/motorSimIOC/configure/CONFIG_SITE | 44 + iocs/motorSimIOC/configure/Makefile | 8 + iocs/motorSimIOC/configure/RELEASE | 8 + iocs/motorSimIOC/configure/RULES | 6 + iocs/motorSimIOC/configure/RULES.ioc | 2 + iocs/motorSimIOC/configure/RULES_DIRS | 2 + iocs/motorSimIOC/configure/RULES_TOP | 3 + iocs/motorSimIOC/iocBoot/Makefile | 6 + iocs/motorSimIOC/iocBoot/iocMotorSim/Makefile | 9 + iocs/motorSimIOC/iocBoot/iocMotorSim/README | 24 + .../iocBoot/iocMotorSim/motor.substitutions | 14 + iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd | 24 + .../motorSimIOC/iocBoot/iocMotorSim/st.cmd.Vx | 29 + .../iocBoot/iocMotorSim/st.cmd.unix | 26 + .../iocBoot/iocMotorSim/st.cmd.win32 | 16 + .../iocBoot/iocMotorSim/start_medm | 1 + iocs/motorSimIOC/motorSimApp/Db/Makefile | 22 + iocs/motorSimIOC/motorSimApp/Makefile | 8 + iocs/motorSimIOC/motorSimApp/src/Makefile | 56 + .../motorSimApp/src/motorSimMain.cpp | 23 + motorSimApp/Db/Makefile | 22 + motorSimApp/Makefile | 8 + .../iocsh/EXAMPLE_motorSim.substitutions | 7 + motorSimApp/iocsh/Makefile | 6 + motorSimApp/iocsh/motorSim.iocsh | 34 + motorSimApp/src/Makefile | 63 + motorSimApp/src/devMotorSim.c | 331 +++++ motorSimApp/src/drvMotorSim.c | 822 ++++++++++ motorSimApp/src/drvMotorSim.h | 13 + motorSimApp/src/motorSimDriver.cpp | 558 +++++++ motorSimApp/src/motorSimDriver.h | 85 ++ motorSimApp/src/motorSimMain.cpp | 23 + motorSimApp/src/motorSimRegister.cc | 40 + motorSimApp/src/motorSimSupport.dbd | 4 + motorSimApp/src/motorSimTest.db | 60 + motorSimApp/src/motorSimTest.src | 38 + motorSimApp/src/route.c | 1320 +++++++++++++++++ motorSimApp/src/route.h | 69 + 55 files changed, 4155 insertions(+) create mode 100644 .gitattributes create mode 100644 .gitignore create mode 100644 GNUmakefile create mode 100644 Makefile create mode 100644 README.md create mode 100644 configure/CONFIG create mode 100644 configure/CONFIG_SITE create mode 100644 configure/Makefile create mode 100644 configure/RELEASE create mode 100644 configure/RULES create mode 100644 configure/RULES.ioc create mode 100644 configure/RULES_DIRS create mode 100644 configure/RULES_TOP create mode 100644 docs/RELEASE.md create mode 100644 iocs/Makefile create mode 100644 iocs/motorSimIOC/Makefile create mode 100644 iocs/motorSimIOC/configure/CONFIG create mode 100644 iocs/motorSimIOC/configure/CONFIG_SITE create mode 100644 iocs/motorSimIOC/configure/Makefile create mode 100644 iocs/motorSimIOC/configure/RELEASE create mode 100644 iocs/motorSimIOC/configure/RULES create mode 100644 iocs/motorSimIOC/configure/RULES.ioc create mode 100644 iocs/motorSimIOC/configure/RULES_DIRS create mode 100644 iocs/motorSimIOC/configure/RULES_TOP create mode 100644 iocs/motorSimIOC/iocBoot/Makefile create mode 100755 iocs/motorSimIOC/iocBoot/iocMotorSim/Makefile create mode 100644 iocs/motorSimIOC/iocBoot/iocMotorSim/README create mode 100644 iocs/motorSimIOC/iocBoot/iocMotorSim/motor.substitutions create mode 100644 iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd create mode 100644 iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.Vx create mode 100644 iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.unix create mode 100755 iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.win32 create mode 100755 iocs/motorSimIOC/iocBoot/iocMotorSim/start_medm create mode 100644 iocs/motorSimIOC/motorSimApp/Db/Makefile create mode 100644 iocs/motorSimIOC/motorSimApp/Makefile create mode 100644 iocs/motorSimIOC/motorSimApp/src/Makefile create mode 100644 iocs/motorSimIOC/motorSimApp/src/motorSimMain.cpp create mode 100644 motorSimApp/Db/Makefile create mode 100644 motorSimApp/Makefile create mode 100644 motorSimApp/iocsh/EXAMPLE_motorSim.substitutions create mode 100644 motorSimApp/iocsh/Makefile create mode 100644 motorSimApp/iocsh/motorSim.iocsh create mode 100644 motorSimApp/src/Makefile create mode 100644 motorSimApp/src/devMotorSim.c create mode 100644 motorSimApp/src/drvMotorSim.c create mode 100644 motorSimApp/src/drvMotorSim.h create mode 100644 motorSimApp/src/motorSimDriver.cpp create mode 100644 motorSimApp/src/motorSimDriver.h create mode 100644 motorSimApp/src/motorSimMain.cpp create mode 100644 motorSimApp/src/motorSimRegister.cc create mode 100644 motorSimApp/src/motorSimSupport.dbd create mode 100644 motorSimApp/src/motorSimTest.db create mode 100644 motorSimApp/src/motorSimTest.src create mode 100644 motorSimApp/src/route.c create mode 100644 motorSimApp/src/route.h diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..9712a09 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,5 @@ +#Which files need CRLF handling +* text=auto +*.sh eol=lf +*.bat eol=crlf +*.cmd -text diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1263d68 --- /dev/null +++ b/.gitignore @@ -0,0 +1,18 @@ +*~ +O.* +*.swp +*BAK.adl +bin/ +db/ +dbd/ +html/ +include/ +lib/ +templates/ +cdCommands +envPaths +dllPath.bat +auto_settings.sav* +auto_positions.sav* +.ccfxprepdir/ +*.local diff --git a/GNUmakefile b/GNUmakefile new file mode 100644 index 0000000..936d4fb --- /dev/null +++ b/GNUmakefile @@ -0,0 +1,25 @@ +# This build the sinqMotor base class +include /ioc/tools/driver.makefile + +MODULE=motorMotorSim +BUILDCLASSES=Linux +EPICS_VERSIONS=7.0.7 +ARCH_FILTER=RHEL% + +# Specify the version of asynMotor we want to build against +motorBase_VERSION=7.2.2 + +# motorRecord.h will be created from motorRecord.dbd +# install devMotorSoft.dbd into /dbd +DBD += motorSimApp/src/motorSimSupport.dbd + +# Source files to build +SOURCES += motorSimApp/src/route.c +SOURCES += motorSimApp/src/devMotorSim.c +SOURCES += motorSimApp/src/drvMotorSim.c +SOURCES += motorSimApp/src/motorSimDriver.cpp +SOURCES += motorSimApp/src/motorSimRegister.cc + +USR_CFLAGS += -Wall -Wextra -Weffc++ -Wunused-result # -Werror + +# MISCS would be the place to keep the stream device template files diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..7011092 --- /dev/null +++ b/Makefile @@ -0,0 +1,44 @@ +# Makefile at top of application tree +TOP = . +include $(TOP)/configure/CONFIG + +# Directories to build, any order +DIRS += configure +DIRS += $(wildcard *Sup) +DIRS += $(wildcard *App) +DIRS += $(wildcard *Top) +DIRS += $(wildcard iocBoot) + +ifeq ($(BUILD_IOCS), YES) +DIRS += $(wildcard iocs) +endif + +# The build order is controlled by these dependency rules: + +# All dirs except configure depend on configure +$(foreach dir, $(filter-out configure, $(DIRS)), \ + $(eval $(dir)_DEPEND_DIRS += configure)) + +# Any *App dirs depend on all *Sup dirs +$(foreach dir, $(filter %App, $(DIRS)), \ + $(eval $(dir)_DEPEND_DIRS += $(filter %Sup, $(DIRS)))) + +# Any *Top dirs depend on all *Sup and *App dirs +$(foreach dir, $(filter %Top, $(DIRS)), \ + $(eval $(dir)_DEPEND_DIRS += $(filter %Sup %App, $(DIRS)))) + +# iocBoot depends on all *App dirs +iocBoot_DEPEND_DIRS += $(filter %App,$(DIRS)) + +# Add any additional dependency rules here: + +# iocs depend on all *Sup and *App dirs +$(foreach dir, $(filter %iocs, $(DIRS)), \ + $(eval $(dir)_DEPEND_DIRS += $(filter %Sup %App, $(DIRS)))) + +# Only support top-level targets when submodule is built stand-alone +ifeq ($(INSTALL_LOCATION),$(MOTOR)) + include $(TOP)/configure/RULES_DIRS +else + include $(TOP)/configure/RULES_TOP +endif \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..9f59494 --- /dev/null +++ b/README.md @@ -0,0 +1,7 @@ +# PSI mirror of Marche + +This repository is a PSI-internal mirror of commit 6e2f646 of the motorMotorSim repository of epics-motor (https://github.com/epics-motor/motorMotorSim/tree/6e2f64601e5f27ba212fc8e8488ac655cc4bb483). A GNUmakefile has been added which builds the simulated motor driver as a module of the GFA build system. + +## How to build it + +Clone this repository to a suitable location (`git clone https://git.psi.ch/sinq-epics-modules/motormotorsim#`). Afterwards, switch to the directory (`cd motormotorsim`) and run `make install`. diff --git a/configure/CONFIG b/configure/CONFIG new file mode 100644 index 0000000..c1a4703 --- /dev/null +++ b/configure/CONFIG @@ -0,0 +1,29 @@ +# CONFIG - Load build configuration data +# +# Do not make changes to this file! + +# Allow user to override where the build rules come from +RULES = $(EPICS_BASE) + +# RELEASE files point to other application tops +include $(TOP)/configure/RELEASE +-include $(TOP)/configure/RELEASE.$(EPICS_HOST_ARCH).Common +ifdef T_A +-include $(TOP)/configure/RELEASE.Common.$(T_A) +-include $(TOP)/configure/RELEASE.$(EPICS_HOST_ARCH).$(T_A) +endif + +CONFIG = $(RULES)/configure +include $(CONFIG)/CONFIG + +# Override the Base definition: +INSTALL_LOCATION = $(TOP) + +# CONFIG_SITE files contain other build configuration settings +include $(TOP)/configure/CONFIG_SITE +-include $(TOP)/configure/CONFIG_SITE.$(EPICS_HOST_ARCH).Common +ifdef T_A + -include $(TOP)/configure/CONFIG_SITE.Common.$(T_A) + -include $(TOP)/configure/CONFIG_SITE.$(EPICS_HOST_ARCH).$(T_A) +endif + diff --git a/configure/CONFIG_SITE b/configure/CONFIG_SITE new file mode 100644 index 0000000..212485e --- /dev/null +++ b/configure/CONFIG_SITE @@ -0,0 +1,43 @@ +# CONFIG_SITE + +# Make any application-specific changes to the EPICS build +# configuration variables in this file. +# +# Host/target specific settings can be specified in files named +# CONFIG_SITE.$(EPICS_HOST_ARCH).Common +# CONFIG_SITE.Common.$(T_A) +# CONFIG_SITE.$(EPICS_HOST_ARCH).$(T_A) + +# CHECK_RELEASE controls the consistency checking of the support +# applications pointed to by the RELEASE* files. +# Normally CHECK_RELEASE should be set to YES. +# Set CHECK_RELEASE to NO to disable checking completely. +# Set CHECK_RELEASE to WARN to perform consistency checking but +# continue building even if conflicts are found. +CHECK_RELEASE = YES + +# Set this when you only want to compile this application +# for a subset of the cross-compiled target architectures +# that Base is built for. +#CROSS_COMPILER_TARGET_ARCHS = vxWorks-ppc32 + +# To install files into a location other than $(TOP) define +# INSTALL_LOCATION here. +#INSTALL_LOCATION= + +# Set this when the IOC and build host use different paths +# to the install location. This may be needed to boot from +# a Microsoft FTP server say, or on some NFS configurations. +#IOCS_APPL_TOP = + +# For application debugging purposes, override the HOST_OPT and/ +# or CROSS_OPT settings from base/configure/CONFIG_SITE +#HOST_OPT = NO +#CROSS_OPT = NO + +# These allow developers to override the CONFIG_SITE variable +# settings without having to modify the configure/CONFIG_SITE +# file itself. +-include $(TOP)/../CONFIG_SITE.local +-include $(TOP)/configure/CONFIG_SITE.local + diff --git a/configure/Makefile b/configure/Makefile new file mode 100644 index 0000000..69c791d --- /dev/null +++ b/configure/Makefile @@ -0,0 +1,12 @@ +TOP=.. + +include $(TOP)/configure/CONFIG + +ifeq ($(INSTALL_CONFIG),$(MOTOR)/configure) +# Don't overwrite Motor's configure files +INSTALL_CONFIG = +endif + +TARGETS = $(CONFIG_TARGETS) + +include $(TOP)/configure/RULES diff --git a/configure/RELEASE b/configure/RELEASE new file mode 100644 index 0000000..561a5e7 --- /dev/null +++ b/configure/RELEASE @@ -0,0 +1,7 @@ +# RELEASE - Location of external support modules + +# Use motor/module's generated release file when buidling inside motor +-include $(TOP)/../RELEASE.$(EPICS_HOST_ARCH).local +# Use motorMotorSim's RELEASE.local when building outside motor +-include $(TOP)/configure/RELEASE.local + diff --git a/configure/RULES b/configure/RULES new file mode 100644 index 0000000..6d56e14 --- /dev/null +++ b/configure/RULES @@ -0,0 +1,6 @@ +# RULES + +include $(CONFIG)/RULES + +# Library should be rebuilt because LIBOBJS may have changed. +$(LIBNAME): ../Makefile diff --git a/configure/RULES.ioc b/configure/RULES.ioc new file mode 100644 index 0000000..901987c --- /dev/null +++ b/configure/RULES.ioc @@ -0,0 +1,2 @@ +#RULES.ioc +include $(CONFIG)/RULES.ioc diff --git a/configure/RULES_DIRS b/configure/RULES_DIRS new file mode 100644 index 0000000..3ba269d --- /dev/null +++ b/configure/RULES_DIRS @@ -0,0 +1,2 @@ +#RULES_DIRS +include $(CONFIG)/RULES_DIRS diff --git a/configure/RULES_TOP b/configure/RULES_TOP new file mode 100644 index 0000000..d09d668 --- /dev/null +++ b/configure/RULES_TOP @@ -0,0 +1,3 @@ +#RULES_TOP +include $(CONFIG)/RULES_TOP + diff --git a/docs/RELEASE.md b/docs/RELEASE.md new file mode 100644 index 0000000..547ce30 --- /dev/null +++ b/docs/RELEASE.md @@ -0,0 +1,33 @@ +# motorMotorSim Releases + +## __R1-1 (2020-05-12)__ +R1-1 is a release based on the master branch. + +### Changes since R1-0 + +#### New features +* None + +#### Modifications to existing features +* Commit [d61a030](https://github.com/epics-motor/motorMotorSim/commit/d61a03052df7fd066d30d88c733022ea3ce1e12b): iocsh files are now installed to the top-level directory + +#### Bug fixes +* Pull request [#2](https://github.com/epics-motor/motorMotorSim/pull/2): Fix setting home bit after home operation +* Commit [ed21eac](https://github.com/epics-motor/motorMotorSim/commit/ed21eac09615ec5bb8bde44a13df28cd651b0f35): Include ``$(MOTOR)/modules/RELEASE.$(EPICS_HOST_ARCH).local`` instead of ``$(MOTOR)/configure/RELEASE`` + +## __R1-0 (2019-04-18)__ +R1-0 is a release based on the master branch. + +### Changes since motor-6-11 + +motorMotorSim is now a standalone module, as well as a submodule of [motor](https://github.com/epics-modules/motor) + +#### New features +* motorMotorSim can be built outside of the motor directory +* motorMotorSim has a dedicated example IOC that can be built outside of motorMotorSim + +#### Modifications to existing features +* None + +#### Bug fixes +* None diff --git a/iocs/Makefile b/iocs/Makefile new file mode 100644 index 0000000..f2af43a --- /dev/null +++ b/iocs/Makefile @@ -0,0 +1,25 @@ +TOP = .. +include $(TOP)/configure/CONFIG + +DIRS += motorSimIOC + +include $(TOP)/configure/RULES_TOP + +uninstallTargets = $(foreach dir, $(DIRS), $(dir)$(DIVIDER)uninstall) +uninstall: $(uninstallTargets) +define UNINSTALL_template +$(1)$(DIVIDER)uninstall: + $(MAKE) -C $(1) uninstall +endef +$(foreach dir, $(DIRS), $(eval $(call UNINSTALL_template,$(dir)))) +.PHONY: uninstall $(uninstallTargets) + +realuninstallTargets = $(foreach dir, $(DIRS), $(dir)$(DIVIDER)realuninstall) +realuninstall: $(realuninstallTargets) +define REALUNINSTALL_template +$(1)$(DIVIDER)realuninstall: + $(MAKE) -C $(1) realuninstall +endef +$(foreach dir, $(DIRS), $(eval $(call REALUNINSTALL_template,$(dir)))) +.PHONY: realuninstall $(realuninstallTargets) + diff --git a/iocs/motorSimIOC/Makefile b/iocs/motorSimIOC/Makefile new file mode 100644 index 0000000..19c9068 --- /dev/null +++ b/iocs/motorSimIOC/Makefile @@ -0,0 +1,31 @@ +# Makefile at top of application tree +TOP = . +include $(TOP)/configure/CONFIG + +# Directories to build, any order +DIRS += configure +DIRS += $(wildcard *Sup) +DIRS += $(wildcard *App) +DIRS += $(wildcard *Top) +DIRS += $(wildcard iocBoot) + +# The build order is controlled by these dependency rules: + +# All dirs except configure depend on configure +$(foreach dir, $(filter-out configure, $(DIRS)), \ + $(eval $(dir)_DEPEND_DIRS += configure)) + +# Any *App dirs depend on all *Sup dirs +$(foreach dir, $(filter %App, $(DIRS)), \ + $(eval $(dir)_DEPEND_DIRS += $(filter %Sup, $(DIRS)))) + +# Any *Top dirs depend on all *Sup and *App dirs +$(foreach dir, $(filter %Top, $(DIRS)), \ + $(eval $(dir)_DEPEND_DIRS += $(filter %Sup %App, $(DIRS)))) + +# iocBoot depends on all *App dirs +iocBoot_DEPEND_DIRS += $(filter %App,$(DIRS)) + +# Add any additional dependency rules here: + +include $(TOP)/configure/RULES_TOP diff --git a/iocs/motorSimIOC/configure/CONFIG b/iocs/motorSimIOC/configure/CONFIG new file mode 100644 index 0000000..c1a4703 --- /dev/null +++ b/iocs/motorSimIOC/configure/CONFIG @@ -0,0 +1,29 @@ +# CONFIG - Load build configuration data +# +# Do not make changes to this file! + +# Allow user to override where the build rules come from +RULES = $(EPICS_BASE) + +# RELEASE files point to other application tops +include $(TOP)/configure/RELEASE +-include $(TOP)/configure/RELEASE.$(EPICS_HOST_ARCH).Common +ifdef T_A +-include $(TOP)/configure/RELEASE.Common.$(T_A) +-include $(TOP)/configure/RELEASE.$(EPICS_HOST_ARCH).$(T_A) +endif + +CONFIG = $(RULES)/configure +include $(CONFIG)/CONFIG + +# Override the Base definition: +INSTALL_LOCATION = $(TOP) + +# CONFIG_SITE files contain other build configuration settings +include $(TOP)/configure/CONFIG_SITE +-include $(TOP)/configure/CONFIG_SITE.$(EPICS_HOST_ARCH).Common +ifdef T_A + -include $(TOP)/configure/CONFIG_SITE.Common.$(T_A) + -include $(TOP)/configure/CONFIG_SITE.$(EPICS_HOST_ARCH).$(T_A) +endif + diff --git a/iocs/motorSimIOC/configure/CONFIG_SITE b/iocs/motorSimIOC/configure/CONFIG_SITE new file mode 100644 index 0000000..4abcdfd --- /dev/null +++ b/iocs/motorSimIOC/configure/CONFIG_SITE @@ -0,0 +1,44 @@ +# CONFIG_SITE + +# Make any application-specific changes to the EPICS build +# configuration variables in this file. +# +# Host/target specific settings can be specified in files named +# CONFIG_SITE.$(EPICS_HOST_ARCH).Common +# CONFIG_SITE.Common.$(T_A) +# CONFIG_SITE.$(EPICS_HOST_ARCH).$(T_A) + +# CHECK_RELEASE controls the consistency checking of the support +# applications pointed to by the RELEASE* files. +# Normally CHECK_RELEASE should be set to YES. +# Set CHECK_RELEASE to NO to disable checking completely. +# Set CHECK_RELEASE to WARN to perform consistency checking but +# continue building even if conflicts are found. +CHECK_RELEASE = YES + +# Set this when you only want to compile this application +# for a subset of the cross-compiled target architectures +# that Base is built for. +#CROSS_COMPILER_TARGET_ARCHS = vxWorks-ppc32 + +# To install files into a location other than $(TOP) define +# INSTALL_LOCATION here. +#INSTALL_LOCATION= + +# Set this when the IOC and build host use different paths +# to the install location. This may be needed to boot from +# a Microsoft FTP server say, or on some NFS configurations. +#IOCS_APPL_TOP = + +# For application debugging purposes, override the HOST_OPT and/ +# or CROSS_OPT settings from base/configure/CONFIG_SITE +#HOST_OPT = NO +#CROSS_OPT = NO + +# Include motor's CONFIG_SITE.local when building inside motor +-include $(TOP)/../../../../configure/CONFIG_SITE.local +# Include motorMotorSim's CONFIG_SITE.local when building inside motorMotorSim +-include $(TOP)/../../configure/CONFIG_SITE.local +# Use motorSimIOC's CONFIG_SITE.local +-include $(TOP)/configure/CONFIG_SITE.local + diff --git a/iocs/motorSimIOC/configure/Makefile b/iocs/motorSimIOC/configure/Makefile new file mode 100644 index 0000000..9254309 --- /dev/null +++ b/iocs/motorSimIOC/configure/Makefile @@ -0,0 +1,8 @@ +TOP=.. + +include $(TOP)/configure/CONFIG + +TARGETS = $(CONFIG_TARGETS) +CONFIGS += $(subst ../,,$(wildcard $(CONFIG_INSTALLS))) + +include $(TOP)/configure/RULES diff --git a/iocs/motorSimIOC/configure/RELEASE b/iocs/motorSimIOC/configure/RELEASE new file mode 100644 index 0000000..cc3c62b --- /dev/null +++ b/iocs/motorSimIOC/configure/RELEASE @@ -0,0 +1,8 @@ +# RELEASE - Location of external support modules + +# Use motor/module's generated release file when buidling inside motor +-include $(TOP)/../../../RELEASE.$(EPICS_HOST_ARCH).local +# Use motorMotorSim's release file when building inside motorMotorSim, but outside motor +-include $(TOP)/../../configure/RELEASE.local +# Use motorSimIOC's RELEASE.local when building outside motorMotorSim +-include $(TOP)/configure/RELEASE.local diff --git a/iocs/motorSimIOC/configure/RULES b/iocs/motorSimIOC/configure/RULES new file mode 100644 index 0000000..6d56e14 --- /dev/null +++ b/iocs/motorSimIOC/configure/RULES @@ -0,0 +1,6 @@ +# RULES + +include $(CONFIG)/RULES + +# Library should be rebuilt because LIBOBJS may have changed. +$(LIBNAME): ../Makefile diff --git a/iocs/motorSimIOC/configure/RULES.ioc b/iocs/motorSimIOC/configure/RULES.ioc new file mode 100644 index 0000000..901987c --- /dev/null +++ b/iocs/motorSimIOC/configure/RULES.ioc @@ -0,0 +1,2 @@ +#RULES.ioc +include $(CONFIG)/RULES.ioc diff --git a/iocs/motorSimIOC/configure/RULES_DIRS b/iocs/motorSimIOC/configure/RULES_DIRS new file mode 100644 index 0000000..3ba269d --- /dev/null +++ b/iocs/motorSimIOC/configure/RULES_DIRS @@ -0,0 +1,2 @@ +#RULES_DIRS +include $(CONFIG)/RULES_DIRS diff --git a/iocs/motorSimIOC/configure/RULES_TOP b/iocs/motorSimIOC/configure/RULES_TOP new file mode 100644 index 0000000..d09d668 --- /dev/null +++ b/iocs/motorSimIOC/configure/RULES_TOP @@ -0,0 +1,3 @@ +#RULES_TOP +include $(CONFIG)/RULES_TOP + diff --git a/iocs/motorSimIOC/iocBoot/Makefile b/iocs/motorSimIOC/iocBoot/Makefile new file mode 100644 index 0000000..91e47d0 --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/Makefile @@ -0,0 +1,6 @@ +TOP = .. +include $(TOP)/configure/CONFIG +DIRS += $(wildcard *ioc*) +DIRS += $(wildcard as*) +include $(CONFIG)/RULES_DIRS + diff --git a/iocs/motorSimIOC/iocBoot/iocMotorSim/Makefile b/iocs/motorSimIOC/iocBoot/iocMotorSim/Makefile new file mode 100755 index 0000000..e4ee12d --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/iocMotorSim/Makefile @@ -0,0 +1,9 @@ +TOP = ../.. +include $(TOP)/configure/CONFIG +#ARCH = linux-x86_64 +#ARCH = vxWorks-ppc32 +ARCH = $(EPICS_HOST_ARCH) +TARGETS = cdCommands +TARGETS += envPaths +include $(TOP)/configure/RULES.ioc + diff --git a/iocs/motorSimIOC/iocBoot/iocMotorSim/README b/iocs/motorSimIOC/iocBoot/iocMotorSim/README new file mode 100644 index 0000000..f7ddad1 --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/iocMotorSim/README @@ -0,0 +1,24 @@ +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/iocs/motorSimIOC/iocBoot/iocMotorSim/motor.substitutions b/iocs/motorSimIOC/iocBoot/iocMotorSim/motor.substitutions new file mode 100644 index 0000000..c01e659 --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/iocMotorSim/motor.substitutions @@ -0,0 +1,14 @@ +file "$(MOTOR)/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/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd b/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd new file mode 100644 index 0000000..824e2b2 --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd @@ -0,0 +1,24 @@ +#!../../bin/linux-x86_64/motorSim + +< envPaths + +cd "${TOP}" + +## Register all support components +dbLoadDatabase "dbd/motorSim.dbd" +motorSim_registerRecordDeviceDriver pdbbase + +cd "${TOP}/iocBoot/${IOC}" + + +## motorUtil (allstop & alldone) +dbLoadRecords("$(MOTOR)/db/motorUtil.db", "P=motorSim:") + +## + +iocInit + +## motorUtil (allstop & alldone) +motorUtilInit("motorSim:") + +# Boot complete diff --git a/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.Vx b/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.Vx new file mode 100644 index 0000000..6d29b33 --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.Vx @@ -0,0 +1,29 @@ +# 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,"motorSim.munch") + +cd startup +dbLoadDatabase("$(TOP)/dbd/motorSim.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/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.unix b/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.unix new file mode 100644 index 0000000..f7ef9e6 --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.unix @@ -0,0 +1,26 @@ +# The is the ASYN example for communication to 4 simulated motors +# "#!" marks lines that can be uncommented. + +< envPaths + +dbLoadDatabase("$(TOP)/dbd/motorSim.dbd") +motorSim_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/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.win32 b/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.win32 new file mode 100755 index 0000000..aae512b --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/iocMotorSim/st.cmd.win32 @@ -0,0 +1,16 @@ +# The is the ASYN example for communication to 4 simulated motors +# "#!" marks lines that can be uncommented. + +< envPaths + +dbLoadDatabase("$(TOP)/dbd/motorSim.dbd") +motorSim_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/iocs/motorSimIOC/iocBoot/iocMotorSim/start_medm b/iocs/motorSimIOC/iocBoot/iocMotorSim/start_medm new file mode 100755 index 0000000..9cd0cba --- /dev/null +++ b/iocs/motorSimIOC/iocBoot/iocMotorSim/start_medm @@ -0,0 +1 @@ +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/iocs/motorSimIOC/motorSimApp/Db/Makefile b/iocs/motorSimIOC/motorSimApp/Db/Makefile new file mode 100644 index 0000000..983981d --- /dev/null +++ b/iocs/motorSimIOC/motorSimApp/Db/Makefile @@ -0,0 +1,22 @@ +TOP=../.. +include $(TOP)/configure/CONFIG +#---------------------------------------- +# ADD MACRO DEFINITIONS AFTER THIS LINE + +#---------------------------------------------------- +# Optimization of db files using dbst (DEFAULT: NO) +#DB_OPT = YES + +#---------------------------------------------------- +# Create and install (or just install) into /db +# databases, templates, substitutions like this +#DB += xxx.db + +#---------------------------------------------------- +# If .db template is not named *.template add +# _template = + +include $(TOP)/configure/RULES +#---------------------------------------- +# ADD RULES AFTER THIS LINE + diff --git a/iocs/motorSimIOC/motorSimApp/Makefile b/iocs/motorSimIOC/motorSimApp/Makefile new file mode 100644 index 0000000..10e0126 --- /dev/null +++ b/iocs/motorSimIOC/motorSimApp/Makefile @@ -0,0 +1,8 @@ +TOP = .. +include $(TOP)/configure/CONFIG +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *src*)) +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *Src*)) +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *db*)) +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *Db*)) +include $(TOP)/configure/RULES_DIRS + diff --git a/iocs/motorSimIOC/motorSimApp/src/Makefile b/iocs/motorSimIOC/motorSimApp/src/Makefile new file mode 100644 index 0000000..9a3710f --- /dev/null +++ b/iocs/motorSimIOC/motorSimApp/src/Makefile @@ -0,0 +1,56 @@ +TOP=../.. + +include $(TOP)/configure/CONFIG +#---------------------------------------- +# ADD MACRO DEFINITIONS AFTER THIS LINE +#============================= + +# The following are used for debugging messages. +#!USR_CXXFLAGS += -DDEBUG + +#============================= +# Build the 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 + +# Include dbd files from all support applications: +#ifdef ASYN +motorSim_DBD += asyn.dbd +#endif +motorSim_DBD += motorSupport.dbd +motorSim_DBD += motorSimSupport.dbd + +# Add all the support libraries needed by this IOC +motorSim_LIBS += motorSimSupport +motorSim_LIBS += motor +#ifdef ASYN +motorSim_LIBS += asyn +#endif +#ifdef SNCSEQ +motorSim_LIBS += seq pv +#endif + +# motorSim_registerRecordDeviceDriver.cpp derives from motorSim.dbd +motorSim_SRCS += motorSim_registerRecordDeviceDriver.cpp + +# Build the main IOC entry point on workstation OSs. +motorSim_SRCS_DEFAULT += motorSimMain.cpp +motorSim_SRCS_vxWorks += -nil- + +# Add support from base/src/vxWorks if needed +#motorSim_OBJS_vxWorks += $(EPICS_BASE_BIN)/vxComLibrary + +# Finally link to the EPICS Base libraries +motorSim_LIBS += $(EPICS_BASE_IOC_LIBS) + +#=========================== + +include $(TOP)/configure/RULES +#---------------------------------------- +# ADD RULES AFTER THIS LINE + diff --git a/iocs/motorSimIOC/motorSimApp/src/motorSimMain.cpp b/iocs/motorSimIOC/motorSimApp/src/motorSimMain.cpp new file mode 100644 index 0000000..03e75f5 --- /dev/null +++ b/iocs/motorSimIOC/motorSimApp/src/motorSimMain.cpp @@ -0,0 +1,23 @@ +/* motorSimMain.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/motorSimApp/Db/Makefile b/motorSimApp/Db/Makefile new file mode 100644 index 0000000..983981d --- /dev/null +++ b/motorSimApp/Db/Makefile @@ -0,0 +1,22 @@ +TOP=../.. +include $(TOP)/configure/CONFIG +#---------------------------------------- +# ADD MACRO DEFINITIONS AFTER THIS LINE + +#---------------------------------------------------- +# Optimization of db files using dbst (DEFAULT: NO) +#DB_OPT = YES + +#---------------------------------------------------- +# Create and install (or just install) into /db +# databases, templates, substitutions like this +#DB += xxx.db + +#---------------------------------------------------- +# If .db template is not named *.template add +# _template = + +include $(TOP)/configure/RULES +#---------------------------------------- +# ADD RULES AFTER THIS LINE + diff --git a/motorSimApp/Makefile b/motorSimApp/Makefile new file mode 100644 index 0000000..3ba4d08 --- /dev/null +++ b/motorSimApp/Makefile @@ -0,0 +1,8 @@ +TOP = .. +include $(TOP)/configure/CONFIG +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *src*)) +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *Src*)) +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *db*)) +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *Db*)) +DIRS := $(DIRS) $(filter-out $(DIRS), $(wildcard *iocsh*)) +include $(TOP)/configure/RULES_DIRS diff --git a/motorSimApp/iocsh/EXAMPLE_motorSim.substitutions b/motorSimApp/iocsh/EXAMPLE_motorSim.substitutions new file mode 100644 index 0000000..4ce6874 --- /dev/null +++ b/motorSimApp/iocsh/EXAMPLE_motorSim.substitutions @@ -0,0 +1,7 @@ +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/motorSimApp/iocsh/Makefile b/motorSimApp/iocsh/Makefile new file mode 100644 index 0000000..9628c6d --- /dev/null +++ b/motorSimApp/iocsh/Makefile @@ -0,0 +1,6 @@ +TOP = ../.. +include $(TOP)/configure/CONFIG + +IOCSH += EXAMPLE_motorSim.substitutions + +include $(TOP)/configure/RULES diff --git a/motorSimApp/iocsh/motorSim.iocsh b/motorSimApp/iocsh/motorSim.iocsh new file mode 100644 index 0000000..80eb0ad --- /dev/null +++ b/motorSimApp/iocsh/motorSim.iocsh @@ -0,0 +1,34 @@ +# ### 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/motorSimApp/src/Makefile b/motorSimApp/src/Makefile new file mode 100644 index 0000000..cfde6f1 --- /dev/null +++ b/motorSimApp/src/Makefile @@ -0,0 +1,63 @@ +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/motorSimApp/src/devMotorSim.c b/motorSimApp/src/devMotorSim.c new file mode 100644 index 0000000..f8889e3 --- /dev/null +++ b/motorSimApp/src/devMotorSim.c @@ -0,0 +1,331 @@ +/* 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/motorSimApp/src/drvMotorSim.c b/motorSimApp/src/drvMotorSim.c new file mode 100644 index 0000000..d28abe7 --- /dev/null +++ b/motorSimApp/src/drvMotorSim.c @@ -0,0 +1,822 @@ +/* +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/motorSimApp/src/drvMotorSim.h b/motorSimApp/src/drvMotorSim.h new file mode 100644 index 0000000..e1b26ed --- /dev/null +++ b/motorSimApp/src/drvMotorSim.h @@ -0,0 +1,13 @@ +#ifndef DRV_MOTOR_SIM_H +#define DRV_MOTOR_SIM_H + +#ifdef __cplusplus +extern "C" { +#endif + +void motorSimCreate( int card, int axis, int hiLimit, int lowLimit, int home, int nCards, int nAxes, int startPosn ); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/motorSimApp/src/motorSimDriver.cpp b/motorSimApp/src/motorSimDriver.cpp new file mode 100644 index 0000000..29dc73e --- /dev/null +++ b/motorSimApp/src/motorSimDriver.cpp @@ -0,0 +1,558 @@ +/* +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; + homed_ = 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; + homed_ = 0; + 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; + homed_ = 0; + 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; + homed_ = 1; + 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_->motorStatusHomed_, (homed_ == 1)); + 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/motorSimApp/src/motorSimDriver.h b/motorSimApp/src/motorSimDriver.h new file mode 100644 index 0000000..5c8dcba --- /dev/null +++ b/motorSimApp/src/motorSimDriver.h @@ -0,0 +1,85 @@ +/* +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 homed_; + 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/motorSimApp/src/motorSimMain.cpp b/motorSimApp/src/motorSimMain.cpp new file mode 100644 index 0000000..ab26c29 --- /dev/null +++ b/motorSimApp/src/motorSimMain.cpp @@ -0,0 +1,23 @@ +/* motorMain.cpp */ +/* Author: Marty Kraimer Date: 17MAR2000 */ + +#include +#include +#include +#include +#include + +#include "epicsExit.h" +#include "epicsThread.h" +#include "iocsh.h" + +int main(int argc,char *argv[]) +{ + if(argc>=2) { + iocsh(argv[1]); + epicsThreadSleep(.2); + } + iocsh(NULL); + epicsExit(0); + return(0); +} diff --git a/motorSimApp/src/motorSimRegister.cc b/motorSimApp/src/motorSimRegister.cc new file mode 100644 index 0000000..b3c2238 --- /dev/null +++ b/motorSimApp/src/motorSimRegister.cc @@ -0,0 +1,40 @@ +#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/motorSimApp/src/motorSimSupport.dbd b/motorSimApp/src/motorSimSupport.dbd new file mode 100644 index 0000000..f6b21fd --- /dev/null +++ b/motorSimApp/src/motorSimSupport.dbd @@ -0,0 +1,4 @@ +device(motor,VME_IO,devMotorSim,"Motor Simulation") +driver(motorSim) +registrar(motorSimRegister) +registrar(motorSimDriverRegister) diff --git a/motorSimApp/src/motorSimTest.db b/motorSimApp/src/motorSimTest.db new file mode 100644 index 0000000..ca8a698 --- /dev/null +++ b/motorSimApp/src/motorSimTest.db @@ -0,0 +1,60 @@ +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/motorSimApp/src/motorSimTest.src b/motorSimApp/src/motorSimTest.src new file mode 100644 index 0000000..244ae2b --- /dev/null +++ b/motorSimApp/src/motorSimTest.src @@ -0,0 +1,38 @@ +#!$(INSTALL)/bin/$(ARCH)/motorSim + +## You may have to change test to something else +## everywhere it appears in this file + +cd "$(INSTALL)" + +# Load binaries on architectures that need to do so. +# VXWORKS_ONLY, LINUX_ONLY and RTEMS_ONLY are macros that resolve +# to a comment symbol on architectures that are not the current +# build architecture, so they can be used liberally to do architecture +# specific things. Alternatively, you can include an architecture +# specific file. +$(VXWORKS_ONLY)ld < bin/$(ARCH)/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/motorSimApp/src/route.c b/motorSimApp/src/route.c new file mode 100644 index 0000000..c093f4a --- /dev/null +++ b/motorSimApp/src/route.c @@ -0,0 +1,1320 @@ +#include +#include +#include /* For definition of the NULL pointer! */ +#include /* For definition of malloc */ +#include + +#define LOCAL static + +typedef struct path_str +{ + double dist; + double vi; + double vf; + double v2; + double t1; + double t2; + double t3; + double t4; + double T; +} path_t; + +typedef struct route_str +{ + route_pars_t pars; + route_demand_t demand; + path_t path[NUM_AXES]; + route_demand_t endp; +} route_t; + +typedef enum +{ + V2 = 1, + T = 2, + T2 = 4, + T4 = 8 +} route_unknown_t; + +#define ZERO_SIZE 2 +#define IS_ZERO(a, scale) (fabs((a)) <= fabs(ZERO_SIZE*DBL_EPSILON*(scale))) +#define DR2D (180.0/3.141592654) + + +/*+ r o u t e D e m a n d + + Function Name: routeDemand + + Function: Returns the position and velocity for a particular time on a path. + + Description: + This function returns the position and velocity at a given time of something + following the specified path. + + Call: + status = routeDemand( &path_t, t, &posn, &vel ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) path (path_t *) Path structure. + (<) t (double) Time at which to find the position and velocity + (>) demand (route_axis_demand_t *) Position and velocit at time t. + + Returns: + status (route_status_t) Always ROUTE__OK + + Author: Nick Rees + +*- + + History: +*/ + +LOCAL route_status_t routeDemand(path_t * path, double t, route_axis_demand_t * demand ) +{ + double accel1, accel2; + + + if (path->t1 != 0) accel1 = (path->v2 - path->vi) / path->t1; + else accel1 = 0; + if (path->t3 != 0) accel2 = (path->vf - path->v2) / path->t3; + else accel2 = 0; + + + if (t >= -path->t4) + { + demand->v = path->vf; + demand->p = path->vf * t; + } + else + { + t = t + path->t4; + demand->p = -path->vf * path->t4; + + if (t >= -path->t3) + { + demand->v = path->vf + accel2 * t; + demand->p += 0.5 * (demand->v + path->vf) * t; + } + else + { + t = t + path->t3; + demand->p -= 0.5 * (path->v2 + path->vf) * path->t3; + + if (t >= -path->t2) + { + demand->v = path->v2; + demand->p += path->v2 * t; + } + else + { + t = t + path->t2; + demand->p -= path->v2 * path->t2; + + if (t >= -path->t1) + { + demand->v = path->v2 + accel1 * t; + demand->p += 0.5 * (demand->v + path->v2) * t; + } + else + { + demand->v = path->vi; + demand->p += 0.5 * (path->vi + path->v2) * path->t2 + (t+path->t1)*path->vi; + } + } + } + } + +/* + + if (t <= path->t1) + { + demand->v = path->vi + accel1 * t; + demand->p = 0.5 * (path->vi + demand->v) * t; + } + else if (t <= (path->t1 + path->t2)) + { + demand->v = path->v2; + demand->p = 0.5 * (path->vi + path->v2) * path->t1 + demand->v * (t - path->t1); + } + else if (t <= (path->t1 + path->t2 + path->t3)) + { + demand->v = path->v2 + accel2 * (t - (path->t1 + path->t2)); + demand->p = (0.5 * (path->vi + path->v2) * path->t1 + path->v2 * path->t2 + + 0.5 * (path->v2 + demand->v) * (t - (path->t1 + path->t2))); + } + else + { + demand->v = path->vf; + demand->p = path->dist + path->vf * (t - path->T); + } +*/ +/* else if (t <= (path->T - path->t4)) + { + demand->v = path->vf - accel2 * (path->T - path->t4 - t); + demand->p = (path->dist - path->vf*path->t4 - + 0.5 * (path->vf + demand->v) * (path->T - path->t4 - t)); + } + else + { + demand->v = path->vf; + demand->p = path->dist + path->vf * (t - path->T); + } +*/ + return ROUTE__OK; +} + + +/*+ r o u t e F i n d W h i c h V 2 S q r t + + Function Name: routeFindWhichV2Sqrt + + Function: Returns the correct quadratic term in the path problem + + Description: + This function returns the correct solution to the second phase coast velocity + quadratic, given the linear and square root terms. + + Call: + status = routeFindWhichV2Sqrt( &path, Ai, lin_term, sqrt_term ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (!) path (path_t *) Path structure. + (<) Ai (double) Phase 1 cceleration. + (<) lin_term (double) Linear term of quadratic solution + (<) sqrt_term (route_unknown_t) Square root term of quadratic solution. + + Returns: + status (route_status_t) ROUTE__OK or ROUTE__NEGSQRT + + Author: Nick Rees +*- + History: +*/ + +LOCAL route_status_t routeFindWhichV2Sqrt( path_t * path, double Ai, + double lin_term, double sqrt_term, int unknown ) +{ + route_status_t status = ROUTE__OK; + +/* if (sqrt_term < -1e-10) + { + status = ROUTE__NEGSQRT; + } + else +*/ { + if (sqrt_term < 0) sqrt_term = 0; + + sqrt_term = sqrt( sqrt_term ); + + if (unknown == T2 ) + { + if ( Ai > 0 ) path->v2 = lin_term - sqrt_term; + else path->v2 = lin_term + sqrt_term; + } + else + { + if ( Ai > 0 ) path->v2 = lin_term + sqrt_term; + else path->v2 = lin_term - sqrt_term; + } + + } + + return status; +} + + +/*+ r o u t e F i n d P a t h + + Function Name: routeFindPath + + Function: Solves the general 4-Phase path problem given a variety of unknowns. + + Description: + + This function finds a path from a given position and velocity to a final + position and velocity via a four stage motion. + + - The first stage is a constant acceleration at +/- Amax, a fixed maximum + acceleration. + - The second stage is a coast at a constant velocity. + - The third stage is a decelleration to the final velocity at the maximum + acceleration. + - The final stage is a coast at the final velocity to the final position. + + This is equivalent to the solving the following four equations: + + distance = 0.5*(vf+v2)*t1 + v2*t2 + 0.5*(vf+v2)*t3 + vf*t4 + t2 = |(v2-vi)/Amax| + t3 = |(v2-vf)/Amax| + T = t1 + t2 + t3 + t4 + |Ai| = Amax + |Af| = Amax + + Where: + + - t1, t2, t3, and t4 are the times for each stage of the motion, + - vi, v2 and vf are the initial, stage 2 and final velocities respectively, + - T is the total time, + - Ai and Af are the initial and final accelerations respectively and + - Amax is the acceleration. + + We also are subject to the constraints t1, t2, t3, t4 > 0. + + We assume we are always given distance, |Amax|, vi, vf and |Vmax| and are + never given t1 and t3. Since we have basically 4 equations we must have 4 + unknowns, which means we also must know 2 of v2, t2, t4 and T. However, + there are sometimes 2 solutions and we also need to know the sign of Amax + (the sign of the initial acceleration). To resolve this we must use the + positivity constraints and the only way of doing this is to test all + possible cases. This makes the routine fairly messy, but I don't know a + way around this. + + In addition, it is also messy since if v2 is unknown, the answer is quadratic + in v2, and both cases in the quadratic have to be decided. + + In the case of ambiguity, the minimum to total time is taken. If two answers + have the same total time, the one with the longest final coast time is + selected. + + The mathematics are solved in the JCMT TCS design note TCS/DN/10 + + Call: + status = routeFindPath( &path, accel, unknowns ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (!) path (path_t *) Path structure. + (<) accel (double) Acceleration. + (<) unknowns (int) Bit mask of unknowns + + Returns: + status (route_status_t) Status + + Author: Nick Rees +*- + History: +*/ + +LOCAL route_status_t routeFindPath( path_t * path, + double accel, + int unknowns ) +{ + double sqrt_term, lin_term, Ai; + route_status_t status = ROUTE__OK; + + if (accel <= 0.0) return ROUTE__BADPARAM; + + switch (unknowns) + { + case ( V2 | T ): + case ( V2 | T4 ): + case ( V2 | T2 ): + { + /* Calculate: + - the minimum time spent accelerating (min_accel_time), + - the maximum time spent coasting at velocity v2 (max_t2_time) and + - the minimum distance spent on other parts of the route (min_not_t2_dist) */ + + double min_accel_time = fabs((path->vi - path->vf)/accel); + double max_t2_time = path->t2; + double vi_dist, vf_dist, min_not_t2_dist; + + switch ( unknowns & ~ V2 ) + { + case( T ): + { + min_not_t2_dist = 0.5*(path->vi + path->vf)*min_accel_time + path->vf * path->t4; + break; + } + case( T4 ): + { + min_not_t2_dist = 0.5*(path->vi + path->vf)*min_accel_time + path->vf * (path->T - path->t2 - min_accel_time); + break; + } + case (T2 ): + { + min_not_t2_dist = 0.5*(path->vi + path->vf)*min_accel_time + path->vf * path->t4; + max_t2_time = path->T - path->t4 - min_accel_time; + break; + } + default: + return ROUTE__BADPARAM; + } + + /* Calculate the two critical distances corresponding to v2=vi and v2=vf */ + vi_dist = min_not_t2_dist + path->vi * max_t2_time; + vf_dist = min_not_t2_dist + path->vf * max_t2_time; + + /* Now calculate v2 */ + if ( path->dist == vi_dist ) + { + path->v2 = path->vi; + } + else if ( path->dist == vf_dist ) + { + path->v2 = path->vf; + } + else + { + if ( ((path->dist < vi_dist) && (path->dist > vf_dist)) || + ((path->dist > vi_dist) && (path->dist < vf_dist)) ) + { + /* Velocity is intermediate between vi and vf */ + if (path->vf > path->vi) Ai = accel; + else Ai = -accel; + + /* Note: the division by max_t2_time in the following switch block + is OK since if max_t2_time = 0, then vi_dist = vf_dist and it is + impossible for path->dist to be intermediate between the two */ + + switch ( unknowns & ~V2 ) + { + case( T ): + path->v2 = (path->dist + + 0.5*(path->vi*path->vi - path->vf*path->vf)/Ai - + path->vf*path->t4 ) / max_t2_time; + break; + case( T4 ): + path->v2 = (path->dist + + 0.5*(path->vi - path->vf)*(path->vi - path->vf)/Ai - + path->vf*(path->T-path->t2)) / max_t2_time; + break; + case( T2 ): + path->v2 = ((path->dist + + 0.5*(path->vi*path->vi - path->vf*path->vf)/Ai - + path->vf*path->t4) / max_t2_time); + break; + default: + break; + } + } + else + { + /* Distance is outside range between vi_dist and vf_dist */ + + if ((path->dist < vi_dist) && (path->dist < vf_dist)) + { + Ai = -accel; + } + else /*if ((path->dist > vi_dist) && (path->dist > vf_dist))*/ + { + Ai = accel; + + } + switch ( unknowns & ~ V2 ) + { + case( T ): + lin_term = -0.5 * Ai * path->t2; + sqrt_term = 0.5 * path->t2 * Ai; + sqrt_term = (sqrt_term * sqrt_term + + 0.5 * (path->vi * path->vi + path->vf * path->vf) + + Ai * (path->dist - path->vf * path->t4)); + break; + case( T4 ): + lin_term = path->vf - 0.5 * Ai * path->t2; + sqrt_term = 0.5 * Ai * path->t2; + sqrt_term = (sqrt_term * sqrt_term + 0.5 * (path->vi - path->vf) * (path->vi - path->vf) + + Ai * (path->dist - path->vf*path->T)); + break; + case( T2 ): + lin_term = 0.5 * (Ai * (path->T-path->t4) + path->vi + path->vf); + sqrt_term = (lin_term * lin_term - + 0.5 * (path->vi * path->vi + path->vf * path->vf) - + Ai * (path->dist - path->vf * path->t4)); + break; + default: + return ROUTE__BADPARAM; + } + + /* Solve the quadratic for v2 */ + status = routeFindWhichV2Sqrt( path, + Ai, + lin_term, + sqrt_term, + (unknowns & ~V2) ); + } + } + + if (status == ROUTE__OK) + { + path->t1 = fabs( ( path->v2 - path->vi ) / accel ); + path->t3 = fabs( ( path->vf - path->v2 ) / accel ); + if (unknowns & T) path->T = path->t1 + path->t2 + path->t3 + path->t4; + if (unknowns & T4) path->t4 = path->T - (path->t1 + path->t2 + path->t3); + if (unknowns & T2) path->t2 = path->T - (path->t1 + path->t3 + path->t4); + } + break; + } + case ( T | T4 ): + case ( T | T2 ): + case ( T2| T4 ): + { + /* We know v2 - life is much easier since t1 and t3 are trivial to find ... */ + double dist; + + path->t1 = fabs( ( path->v2 - path->vi ) / accel ); + path->t3 = fabs( ( path->vf - path->v2 ) / accel ); + dist = path->dist - 0.5*((path->vi+path->v2)*path->t1 + (path->v2 + path->vf)*path->t3); + + if (unknowns & T ) + { + if ((unknowns & T4) && (path->vf != 0)) + { + path->t4 = (dist - path->v2*path->t2) / path->vf; + } + else if ((unknowns & T2) && (path->v2 != 0)) + { + path->t2 = (dist - path->vf*path->t4) / path->v2; + } + else status = ROUTE__BADPARAM; + + path->T = path->t1 + path->t2 + path->t3 + path->t4; + } + else if ( path->v2 != path->vf ) + { + path->t2 = (dist - path->vf * ( path->T - path->t1 - path->t3 ))/ (path->v2 - path->vf); + path->t4 = path->T - path->t1 - path->t2 - path->t3; + } + else status = ROUTE__BADPARAM; + + break; + } + default: + status = ROUTE__BADPARAM; + } + + /* Now do a test for positive times */ + if (status == ROUTE__OK && + (path->t1 < 0 || path->t2 < 0 || path->t3 < 0 || path->t4 < 0 || path->T < 0)) + { + if (IS_ZERO(path->t1, path->T)) path->t1 = 0; + if (IS_ZERO(path->t2, path->T)) path->t2 = 0; + if (IS_ZERO(path->t3, path->T)) path->t3 = 0; + if (IS_ZERO(path->t4, path->T)) path->t4 = 0; + + if (path->t1 < 0 || path->t2 < 0 || path->t3 < 0 || path->t4 < 0 || path->T < 0) + { + status = ROUTE__NEGTIME; + if (path->t1 < 0) path->t1 = 0; + if (path->t2 < 0) path->t2 = 0; + if (path->t3 < 0) path->t3 = 0; + if (path->t4 < 0) path->t4 = 0; + if (path->T < 0) path->T = 0; + } + } + + if (status == ROUTE__OK && + !IS_ZERO( path->T - (path->t1 + path->t2 + path->t3 + path->t4), path->T)) + { + status = ROUTE__NEGTIME; + } + +/* printf( "Path found: T=%g, t1=%g, t2=%g, t3=%g, t4=%g, vi=%g, v2=%g, vf=%g %d %d\n", + path->T, path->t1, path->t2, path->t3, path->t4, + path->vi, path->v2, path->vf, unknowns, status ); */ + + return status; +} + +/*+ r o u t e F i n d P a t h W i t h V m a x + + Function Name: routeFindPathWithVmax + + Function: Solves the 4-Phase path problem with a maximum velocity constraint. + + Description: + + This function finds a path from a given position and velocity to a final + position and velocity via a four stage motion, subject to a maximum velocity. + It uses routeFindPath to determine the paths. + + It initially sets the second stage coast time to zero and attempts to find a + path. If the maximum velocity is less than Vmax, it exits. Otherwise it + restricts the coast velocity (v2) to the maximum velocity and allows the coast + time to vary. + + + Call: + status = routeFindPathWithVmax( &path, accel, vmax, unknowns ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (!) path (path_t *) Path structure. + (<) accel (double) Acceleration. + (<) vmax (double) Maximum velocity + (<) unknowns (route_unknown_t) Unknown - either T, or t4. + + Returns: + status (route_status_t) Status from routeFindPath + + Author: Nick Rees +*- + History: +*/ + + +LOCAL route_status_t routeFindPathWithVmax(path_t * path, double Amax, double Vmax, route_unknown_t unknown) +{ + route_status_t status; + + path->t2 = 0; + status = routeFindPath(path, Amax, V2 | unknown ); + + if ((status == ROUTE__OK) && (fabs(path->v2) > Vmax)) + { + if (path->v2 >= 0.0) + path->v2 = Vmax; + else + path->v2 = -Vmax; + + status = routeFindPath( path, Amax, T2 | unknown ); + } + return status; +} + +/********************************************************************************/ +/* */ +/* External Routines */ +/* */ +/********************************************************************************/ + +/*+ r o u t e N e w + + Function Name: routeNew + + Function: Initialises the routing mechanism + + Description: + This function mallocs and initialises the internal data structures used by + the routing algorithm. + + Call: + route = routeNew( demand, params ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) demand (route_demand_t *) The inital demand for the routing system. All + subsequent routing will derive from this point. + (<) params (route_pars_t *) The initial parameters for routing. + + Returns: + route (ROUTE_ID) Either a pointer to a valid routing structure, or NULL + if an error occurs (Bad parameters or no space available) + + Author: Nick Rees + +*- + + History: +*/ + +ROUTE_ID routeNew( route_demand_t * demand, route_pars_t * params ) +{ + ROUTE_ID route = NULL; + unsigned int i, ok; + + /* Check input parameters */ + ok = (params != NULL && demand != NULL && params->Tsync >= 0); + + for (i=0; inumRoutedAxes && ok; i++) + { + int j = params->routedAxisList[i] - 1; + ok = (params->axis[j].Amax > 0 && + params->axis[j].Vmax > 0 && + params->axis[j].Vmax > fabs(demand->axis[j].v) ); + } + + /* If input parameters are OK, malloc and initialize structure */ + if ( ok && ((route = (route_t *) calloc( sizeof(route_t),1)) != NULL) ) + { + route->pars = *params; + route->endp = *demand; + route->demand = *demand; + for (i=0; inumRoutedAxes; i++) + { + int j = params->routedAxisList[i] - 1; + route->path[j].vi = demand->axis[j].v; + route->path[j].v2 = demand->axis[j].v; + route->path[j].vf = demand->axis[j].v; + } + } + + return route; +} + + +/*+ r o u t e F i n d + + Function Name: routeFind + + Function: Finds a route to a given target and returns the necessary information + + Description: + This function calculates a route from the last demand position to a new endpoint + and returns an appropriate route to this position that lies within the systems + acceleration and velocity constraints. + + Call: + status = routeFind( route, reroute, &endpoint, &nextpoint ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) Route information + (<) reroute (int) Flag set to force a total route recalculation, + including a resynchronisation to the synch + time, if required. It can have one of the following values: + ROUTE_NEW_ROUTE => Force a route recalculation, including + resychronization to the synch time. + ROUTE_NO_NEW_ROUTE => Force routing to be turned off. The next + route position is the end-point and it + will be reached at the nextpoint time. + ROUTE_CALC_ROUTE => Calculate an aceptable route to the next + point. + (!) endpoint (route_demand_t *) On input, the position and velocities in this + must be the final demand for the route. + On output, the time in the structure will be + modified to reflect the actual time the system + will be at the endpoint if the calculated route + is followed. + (!) nextpoint (route_demand_t *) On output, the time in this structure must be + the time for which the next demand is required. + This must be a later time than the previous + time this routine was called for this route. + On output, the positions and velocities in the + structure will be updated to give an appropriate + demand at the given time + + Returns: + status (route_status_t) Always ROUTE__OK + + Author: Nick Rees + +*- + + History: +*/ + +route_status_t routeFind( ROUTE_ID route, route_reroute_t reroute, route_demand_t * endp, route_demand_t * nextp ) +{ + route_status_t status = ROUTE__OK; + route_status_t ret_status = ROUTE__OK; + unsigned int long_path, short_path, i; + int old_path_ok; + + /* If no axes are routed, copy endp into nextp, and return */ + if (route->pars.numRoutedAxes == 0) + { + *nextp = *endp; + + /* Save the previous demands and endpoints */ + route->demand = *nextp; + route->endp = *endp; + return ret_status; + } + + /* If we don't want routing, set the old demand to be a coast from the current endpoint */ + if (reroute == ROUTE_NO_NEW_ROUTE) + { + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + route->demand.axis[j].v = endp->axis[j].v; + route->demand.axis[j].p = endp->axis[j].p - (nextp->T - route->demand.T)*endp->axis[j].v; + } + } + + /* First check whether the previous path is OK to use */ + old_path_ok = (reroute == ROUTE_CALC_ROUTE) && IS_ZERO( (endp->T - route->endp.T), endp->T); + for ( i = 0; i < route->pars.numRoutedAxes && old_path_ok; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + old_path_ok = (IS_ZERO( (endp->axis[j].p - route->endp.axis[j].p), 40.0) && + (fabs(endp->axis[j].v - route->endp.axis[j].v) < (route->pars.axis[j].Vmax*1.0e-10))); +/* old_path_ok = (IS_ZERO( (endp->axis[j].p - route->endp.axis[j].p), 6.0) && */ +/* IS_ZERO( (endp->axis[j].v - route->endp.axis[j].v), route->pars.axis[j].Vmax)); */ + } + + if (!old_path_ok) + { + /* Initialise the Path structures */ + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + route->path[j].dist = endp->axis[j].p - route->demand.axis[j].p; + route->path[j].vi = route->demand.axis[j].v; + route->path[j].vf = endp->axis[j].v; + route->path[j].t2 = 0.0; + route->path[j].t4 = route->pars.Tcoast; + route->path[j].T = endp->T - route->demand.T; + } + + /* Calculate whether we expect to complete this route in the next coast period */ + short_path = (reroute != ROUTE_NEW_ROUTE) && (nextp->T + route->pars.Tcoast >= endp->T); + if (short_path) + { + for (i = 0; ((i < route->pars.numRoutedAxes) && (status == ROUTE__OK)); i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + status = routeFindPathWithVmax(&route->path[j], + route->pars.axis[j].Amax, + route->pars.axis[j].Vmax, + T4 ); + } + } + + if (!short_path || (status != ROUTE__OK)) + { + /* Either we didn't find a route using the above or the time is longer */ + + long_path = 0; + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + route->path[j].t4 = route->pars.Tcoast; + status = routeFindPathWithVmax(&route->path[j], + route->pars.axis[j].Amax, + route->pars.axis[j].Vmax, + T ); + switch (status) + { + case ROUTE__OK: + case ROUTE__NEGSQRT: + case ROUTE__NEGTIME: + break; + default: + return status; + } + + if (route->path[j].T > route->path[long_path].T) long_path = j; + } + + /* Set the time for the path - synchronising it to an integral + number of Tsync units, if required */ + if (route->pars.Tsync > 0) + { + endp->T = ceil((route->demand.T + route->path[long_path].T) / + route->pars.Tsync) * route->pars.Tsync; + route->path[long_path].T = endp->T - route->demand.T; + } + else + { + endp->T = route->demand.T + route->path[long_path].T; + } + + /* Recalculate the paths for the new total path time. */ + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + int j = route->pars.routedAxisList[i] - 1; + route->path[j].T = route->path[long_path].T; + + /* If we are synchronising to an integral number of tic's all + paths have to be recalculated, otherwise all except the + longest path needs recaculating */ + if ( route->pars.Tsync > 0 || j != long_path ) + { + status = routeFindPath(&route->path[j], + route->pars.axis[j].Amax, + T2 | V2 ); + } + + if (status != ROUTE__OK) ret_status = status; + } + } + } + + /* Now all the paths are valid - calculate the demand position at the next time */ + for (i=0; i< route->pars.numRoutedAxes; i++) + { + int j = route->pars.routedAxisList[i] - 1; + status = routeDemand(&route->path[j], + (nextp->T - endp->T), + &(nextp->axis[j]) ); + nextp->axis[j].p += endp->axis[j].p; + if (status != ROUTE__OK) return status; + } + + /* Save the previous demands and endpoints */ + route->demand = *nextp; + route->endp = *endp; + + return ret_status; +} + +/*+ r o u t e D e l e t e + + Function Name: routeDelete + + Function: Deletes all structures associated with the given route + + Description: + This function frees the internal data structures used by the routing + algorithm. + + Call: + (void) routeNew( route ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The ID of the route to be deleted. + + Author: Nick Rees + +*- + + History: +*/ + +void routeDelete( ROUTE_ID route ) +{ + /* Check input parameters */ + free( route ); + return; +} + + +/*+ r o u t e S e t D e m a n d + + Function Name: routeSetDemand + + Function: Re-initialises the current demand of the routing mechanism + + Description: + This function re-initialises the current demand of the routing algorithm. + Note that this function is normally not required because the algorithm + assumes routing is always on, and so the current demand is maintained by + the algorithm internally. + + Call: + route = routeSetDemand( route, demand ) + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The route identifier. + (<) demand (route_demand_t *) The inital demand for the routing system. All + subsequent routing will derive from this point. + + Returns: + status (route_status_t) Always ROUTE__OK - you can set the demand to + anything you like. + + Author: Nick Rees + +*- + + History: +*/ + +route_status_t routeSetDemand( ROUTE_ID route, route_demand_t * demand ) +{ + route->demand = *demand; + + return ROUTE__OK; +} + +/*+ r o u t e S e t P a r a m s + + Function Name: routeSetParams + + Function: Re-initialises the routing mechanism parameters + + Description: + This function allows the user to reinitialise all the routing algorithm + parameteters on the fly. + + Call: + status = routeSetParams( route, params ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The ID of the route to be changed. + (<) params (route_pars_t *) The new routing parameters. + + Returns: + status (route_status_t) ROUTE_BADPARAMS if the times given are + negative or the velocities and accelerations are + negative or zero. Otherwise ROUTE__OK. + + Author: Nick Rees + +*- + + History: +*/ + +route_status_t routeSetParams( ROUTE_ID route, route_pars_t * params ) +{ + unsigned int i, ok; + route_status_t status = ROUTE__OK; + + /* Check input parameters */ + ok = (params != NULL && + params->Tsync >= 0 && + params->Tcoast >= 0 ); + + for (i=0; inumRoutedAxes && ok; i++) + { + int j = params->routedAxisList[i] - 1; + ok = (params->axis[j].Amax > 0 && + params->axis[j].Vmax > 0 && + params->axis[j].Vmax > fabs(route->demand.axis[j].v) ); + } + + /* If input parameters are OK, re-initialize the params structure */ + if ( ok ) + { + route->pars = *params; + } + else + { + status = ROUTE__BADPARAM; + } + return status; +} + +/*+ r o u t e G e t P a r a m s + + Function Name: routeGetParams + + Function: Returns the routing mechanism parameters + + Description: + This function allows the user to read all the routing algorithm + parameteters. + + Call: + status = routeGetParams( route, ¶ms ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The ID of the route to be interrogated. + (>) params (route_pars_t *) The current routing parameters. + + Returns: + status (route_status_t) Always ROUTE__OK. + + Author: Nick Rees + +*- + + History: +*/ + +route_status_t routeGetParams( ROUTE_ID route, route_pars_t * params ) +{ + *params = route->pars; + + return ROUTE__OK; +} + + +/*+ r o u t e G e t N u m R o u t e d A x e s + + Function Name: routeGetNumRoutedAxes + + Function: Returns the number of routed axes + + Description: + This function allows the user to determine how many axes are routed + + Call: + status = routeGetNumRoutedAxes( route, &number ); + + Parameters: + ("<" input, "!" modified, "W" workspace, ">" output) + + (<) route (ROUTE_ID) The ID of the route to be interrogated. + (>) number (unsigned int *) The number of routed axes. + + Returns: + status (route_status_t) Always ROUTE__OK. + + Author: Russell Kackley + +*- + + History: +*/ + +route_status_t routeGetNumRoutedAxes( ROUTE_ID route, unsigned int * number ) +{ + *number = route->pars.numRoutedAxes; + + return ROUTE__OK; +} + + + +/* Test routines */ + +#ifdef TEST_FIND_PATH + +#include + +int main( int * argc, char * argv[] ) +{ + int option = 1; + double value=0.0, distance; + path_t path = {0,0,0,0,0,0,0,0,0}; + double Amax = 0.0; + double Vmax = 0.0; + route_status_t status = 0; + int unknown1 = 1; + int unknown2 = 2; + int unknowns; + char line[128]; + + while ( option != 0 ) + { + printf("\n \ + Choose from: \n \ + 0 - Exit \n \ + 1 - Set Amax (%f) \n \ + 2 - Set Vmax (%f) \n \ + 3 - Set dist (%f) \n \ + 4 - Set vi (%f) \n \ + 5 - Set vf (%f) \n \ + 6 - Set v2 (%f) \n \ + 7 - Set t1 (%f) \n \ + 8 - Set t2 (%f) \n \ + 9 - Set t3 (%f) \n \ + 10 - Set t4 (%f) \n \ + 11 - Set T (%f) \n \ + 12 - Set unknown 1 (1 = v2, 2=T, 3=t2, 4=t4) (%d) \n \ + 13 - Set unknown 2 (1 = v2, 2=T, 3=t2, 4=t4) (%d) \n \ + 14 - Enter all values at once \n \ + \n \ + Enter option : ", + Amax, + Vmax, + path.dist, + path.vi, + path.vf, + path.v2, + path.t1, + path.t2, + path.t3, + path.t4, + path.T, + unknown1, unknown2 ); + + while ( gets( line ) == NULL || line[0] == '#' || (sscanf( line, "%d", &option ) != 1)); + + if (option > 0 && option < 14) + { + printf( "\n Enter value : " ); + while ( gets( line ) == NULL || line[0] == '#' || (sscanf( line, "%lf", &value ) != 1)); + + switch (option) + { + case 1: Amax = value; break; + case 2: Vmax = value; break; + case 3: path.dist = value; break; + case 4: path.vi = value; break; + case 5: path.vf = value; break; + case 6: path.v2 = value; break; + case 7: path.t1 = value; break; + case 8: path.t2 = value; break; + case 9: path.t3 = value; break; + case 10: path.t4 = value; break; + case 11: path.T = value; break; + case 12: unknown1 = (int) value; break; + case 13: unknown2 = (int) value; break; + default: break; + } + } + else if (option == 14) + { + while ( gets( line ) == NULL || line[0] == '#' || + (sscanf( line, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %d", + &Amax, &Vmax, &path.dist, + &path.vi, &path.vf, &path.v2, + &path.t1, &path.t2, &path.t3, &path.t4, &path.T, + &unknown1, &unknown2 ) != 13 )); + } + + unknowns =(0x1 << (int)(unknown1-1)) | (0x1 << (int)(unknown2-1)); + status = routeFindPath( &path, Amax, unknowns ); + distance = (path.t1 * (path.vi+path.v2) / 2 + + path.t2 * path.v2 + + path.t3 * (path.v2+path.vf) / 2 + + path.t4 * path.vf); + + printf("\n" ); + printf(" routeFindPath status = %d\n", (int) status ); + printf(" distance = %.12f\n", distance ); + printf(" distance error = %.12f\n", distance - path.dist ); + } + + return 0; +} + +#endif + +#ifdef TEST + +#include + +#define DAS2R (3.141592654/(3600.0*180.0)) + +route_demand_t route_list[] = +{ + 0.0, -100*DAS2R, 10*DAS2R, 10.0*DAS2R, 0.0, -100*DAS2R, 10*DAS2R, + 0.0, -100*DAS2R, 10*DAS2R, 0.0*DAS2R, 0.0, -100*DAS2R, 10*DAS2R, + 0.0, -100*DAS2R, 10*DAS2R, -10.0*DAS2R, 0.0, -100*DAS2R, 10*DAS2R, + 0.0, -180*3600*DAS2R, 10*DAS2R, -50*3600*DAS2R, 0.0, -180*3600*DAS2R, 10*DAS2R, + 0.0, -180*3600*DAS2R, 10*DAS2R, (-50*3600+10)*DAS2R, 0.0, -180*3600*DAS2R, 10*DAS2R, + 0.0, -180*3600*DAS2R, 10*DAS2R, (-50*3600+20)*DAS2R, 0.0, -180*3600*DAS2R, 10*DAS2R +}; + +#define NUM_DEMANDS (sizeof(route_list)/sizeof(route_demand_t)) + +route_demand_t sky = { 0.0, 0*DAS2R, 10*DAS2R, 0.0*DAS2R, 0.0 }; + +int routeTCSSim( int route_num, route_demand_t * demand, int slewing ) +{ + route_demand_t * this_route = &route_list[route_num % NUM_DEMANDS]; + int i; + + if (slewing) this_route->T = demand->T; + + for (i = 0; i < NUM_AXES; i++ ) + { + demand->axis[i].p = (sky.axis[i].p + + sky.axis[i].v * demand->T + + this_route->axis[i].p + + this_route->axis[i].v * (demand->T - this_route->T)); + demand->axis[i].v = (sky.axis[i].v + this_route->axis[i].v); + } + + return 0; +} + +int route_numbers[] = { 0, 1, 2, 3, 4, 5 }; +double route_times[] = { 10, 10, 10, 60, 60, 60 }; + +#define NUM_ROUTES (sizeof(route_numbers)/sizeof(int)) + +route_demand_t initial_demand = { 0.0, 0.0, 0.0, 0.0, 0.0 0.0, 0.0}; +route_pars_t initial_params = +{ + 3, /* numRoutedAxes */ + 1,2,3, /* routedAxisList */ + 0.05, /* Tsync */ + 1.0, /* Tcoast */ + 0.12*3600.0*DAS2R, /* Amax */ + 0.60*3600.0*DAS2R, /* Vmax */ + 0.12*3600.0*DAS2R, /* Amax */ + 0.60*3600.0*DAS2R, /* Vmax */ + 0.12*3600.0*DAS2R, /* Amax */ + 0.60*3600.0*DAS2R /* Vmax */ +}; + +#define DELTA_T (0.05); + +#include "thi_route_if.h" + +int main( int argc, char * argv[] ) +{ + ROUTE_ID route = routeNew( &initial_demand, &initial_params ); + route_demand_t demand=initial_demand; + route_demand_t next_demand=initial_demand; + double last_route_start = initial_demand.T; + int i, slewing, status, tel_step; + + for ( i = 0; i next_demand.T); + if (!slewing) demand.T = next_demand.T; + } + } + + return 0; +} + +#endif + +void routePrint( ROUTE_ID route, route_reroute_t reroute, route_demand_t * endp, route_demand_t * nextp, FILE * logfile ) +{ + int i; + + fprintf( logfile, "\nreroute %d\n", reroute); + + fprintf( logfile, "\nroute pars struct\n"); + fprintf( logfile, "Tsync, Tcoast %f %f\n", route->pars.Tsync, route->pars.Tcoast ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d Amax, Vmax %f %f\n", i, route->pars.axis[i].Amax * DR2D, route->pars.axis[i].Vmax * DR2D); + } + + fprintf( logfile, "\nroute path struct\n"); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d dist %f vi %f vf %f v2 %f t1 %f t2 %f t3 %f t4 %f T %f \n", i, + route->path[i].dist * DR2D, route->path[i].vi * DR2D, route->path[i].vf * DR2D, route->path[i].v2 * DR2D, + route->path[i].t1, route->path[i].t2, route->path[i].t3, route->path[i].t4, route->path[i].T ); + } + + fprintf( logfile, "\nroute demand struct\n"); + fprintf( logfile, "T %f\n", route->demand.T ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d p, v %f %f\n", i, route->demand.axis[i].p * DR2D, route->demand.axis[i].v * DR2D ); + } + + fprintf( logfile, "\nroute endp struct\n"); + fprintf( logfile, "T %f\n", route->endp.T ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d p, v %f %f\n", i, route->endp.axis[i].p * DR2D, route->endp.axis[i].v * DR2D ); + } + + fprintf( logfile, "\nendp struct\n"); + fprintf( logfile, "T %f\n", endp->T ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d p, v %f %f\n", i, endp->axis[i].p * DR2D, endp->axis[i].v * DR2D ); + } + + fprintf( logfile, "\nnextp struct\n"); + fprintf( logfile, "T %f\n", nextp->T ); + for (i = 0; i < route->pars.numRoutedAxes; i++ ) + { + fprintf( logfile, "Axis %d p, v %f %f\n", i, nextp->axis[i].p * DR2D, nextp->axis[i].v * DR2D ); + } +} + diff --git a/motorSimApp/src/route.h b/motorSimApp/src/route.h new file mode 100644 index 0000000..48b8d73 --- /dev/null +++ b/motorSimApp/src/route.h @@ -0,0 +1,69 @@ +#ifndef __INCrouteLibh +#define __INCrouteLibh + +#ifdef __cplusplus +extern "C" { +#endif + +#define NUM_AXES 3 + +typedef enum +{ + ROUTE_CALC_ROUTE = 0, + ROUTE_NEW_ROUTE = 1, + ROUTE_NO_NEW_ROUTE = 2 +} route_reroute_t; + +typedef enum +{ + ROUTE__OK = 0, + ROUTE__BADROUTE = 1, + ROUTE__BADPARAM = 2, + ROUTE__NEGSQRT = 3, + ROUTE__NEGTIME = 4 +} route_status_t; + +typedef struct route_axis_demand_str +{ + double p; /* Demand position for axis at a given time */ + double v; /* Demand velocity for axis at a given time */ +} route_axis_demand_t; + +typedef struct route_demand_str +{ + double T; /* Time at which demand is valid */ + route_axis_demand_t axis[NUM_AXES]; +} route_demand_t; + +typedef struct route_axis_pars_str +{ + double Amax; /* Maximum acceleration for this axis */ + double Vmax; /* Maximum velocity for this axis */ +} route_axis_pars_t; + +typedef struct route_pars_str +{ + unsigned int numRoutedAxes; /* Number of axes to be routed */ + int routedAxisList[NUM_AXES]; /* List of the axes to be routed */ + double Tsync; /* Synchronisation period for routing */ + double Tcoast; /* End of route coast time for all axes */ + route_axis_pars_t axis[NUM_AXES]; +} route_pars_t; + +typedef struct route_str * ROUTE_ID; + +ROUTE_ID routeNew( route_demand_t * initialDemand, route_pars_t * initial_parameters ); +route_status_t routeFind( ROUTE_ID, route_reroute_t, route_demand_t * end_demand, route_demand_t * next_demand ); +void routePrint( ROUTE_ID route, route_reroute_t reroute, route_demand_t * endp, route_demand_t * nextp, FILE * logfile ); +void routeDelete( ROUTE_ID ); + +route_status_t routeSetDemand( ROUTE_ID, route_demand_t * demand ); +route_status_t routeSetParams( ROUTE_ID, route_pars_t * parameters ); +route_status_t routeGetParams( ROUTE_ID, route_pars_t * parameters ); +route_status_t routeGetNumRoutedAxes( ROUTE_ID route, unsigned int * number ); + +#ifdef __cplusplus +} +#endif + +#endif /* __INCrouteLibh */