From 242e1b8c3772cb6612395bcb787ca9e8d546eff6 Mon Sep 17 00:00:00 2001 From: kmpeters Date: Wed, 7 Jan 2015 22:34:20 +0000 Subject: [PATCH 001/232] Release 6-9 From da350fdce966a336e888f3f2fc3dd6984672f222 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 20 Feb 2015 12:38:46 +0000 Subject: [PATCH 002/232] Initial merge of changes from 6-8-1 Former-commit-id: bde92aa9146606f7d063066acda0357debc88ae2 --- Makefile | 6 +- configure/RELEASE | 29 +++++--- iocBoot/iocNoAsyn/Makefile | 2 +- iocBoot/iocWithAsyn/Makefile | 3 +- iocBoot/iocmotorSim/Makefile | 14 ++++ iocBoot/iocmotorSim/README | 24 +++++++ iocBoot/iocmotorSim/auto_positions.req | 8 +++ iocBoot/iocmotorSim/auto_settings.req | 11 +++ iocBoot/iocmotorSim/motor.substitutions | 14 ++++ .../iocmotorSim/motor.substitutions.allstop | 14 ++++ iocBoot/iocmotorSim/saveData.req | 43 ++++++++++++ iocBoot/iocmotorSim/save_restore.cmd | 68 +++++++++++++++++++ iocBoot/iocmotorSim/st.cmd.Vx | 27 ++++++++ iocBoot/iocmotorSim/st.cmd.allstop | 56 +++++++++++++++ iocBoot/iocmotorSim/st.cmd.motorsim | 49 +++++++++++++ iocBoot/iocmotorSim/st.cmd.sim.inst | 28 ++++++++ iocBoot/iocmotorSim/st.cmd.unix | 27 ++++++++ iocBoot/iocmotorSim/st.cmd.win32 | 14 ++++ iocBoot/iocmotorSim/start_medm | 1 + motorApp/Db/EnsemblePSOFly.db | 8 +-- motorApp/Db/Makefile | 1 + motorApp/Db/SoftMotorEx.db | 4 +- motorApp/Db/motorUtil.db | 50 +++++++++++--- motorApp/Db/motor_positions.req | 2 +- motorApp/Db/motor_settings.req | 2 + motorApp/Db/stop_all.db | 11 +++ motorApp/Db/stopping_motor.db | 31 +++++++++ motorApp/MotorSimSrc/Makefile | 4 ++ motorApp/MotorSrc/devMotorAsyn.c | 2 +- motorApp/MotorSrc/motorUtil.cc | 17 +++-- motorApp/PIGCS2Src/PIasynController.cpp | 1 + motorApp/PhytronSrc/phytronAxisMotor.cpp | 2 + motorExApp/WithAsyn/Makefile | 3 +- 33 files changed, 538 insertions(+), 38 deletions(-) create mode 100755 iocBoot/iocmotorSim/Makefile create mode 100644 iocBoot/iocmotorSim/README create mode 100644 iocBoot/iocmotorSim/auto_positions.req create mode 100644 iocBoot/iocmotorSim/auto_settings.req create mode 100644 iocBoot/iocmotorSim/motor.substitutions create mode 100644 iocBoot/iocmotorSim/motor.substitutions.allstop create mode 100644 iocBoot/iocmotorSim/saveData.req create mode 100644 iocBoot/iocmotorSim/save_restore.cmd create mode 100644 iocBoot/iocmotorSim/st.cmd.Vx create mode 100644 iocBoot/iocmotorSim/st.cmd.allstop create mode 100644 iocBoot/iocmotorSim/st.cmd.motorsim create mode 100644 iocBoot/iocmotorSim/st.cmd.sim.inst create mode 100644 iocBoot/iocmotorSim/st.cmd.unix create mode 100755 iocBoot/iocmotorSim/st.cmd.win32 create mode 100755 iocBoot/iocmotorSim/start_medm create mode 100644 motorApp/Db/stop_all.db create mode 100644 motorApp/Db/stopping_motor.db diff --git a/Makefile b/Makefile index 5bec57217..6b240595c 100644 --- a/Makefile +++ b/Makefile @@ -14,8 +14,8 @@ motorApp_DEPEND_DIRS = configure # 3rd - make clean uninstall # 4th - make -#!DIRS += motorExApp iocBoot -#!motorExApp_DEPEND_DIRS = motorApp -#!iocBoot_DEPEND_DIRS = motorExApp +DIRS += motorExApp iocBoot +motorExApp_DEPEND_DIRS = motorApp +iocBoot_DEPEND_DIRS = motorExApp include $(TOP)/configure/RULES_TOP diff --git a/configure/RELEASE b/configure/RELEASE index 6c1a60a9f..17aa191e4 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -14,16 +14,23 @@ SUPPORT= # If any motor controller communication mechanism besides the VME backplane is # required, then ASYN must be defined. -# Recommended ASYN release: R4-23 -ASYN=$(SUPPORT)/asyn/R4-23 +# Recommended ASYN release: R4-21 +ASYN=$(SUPPORT)/asyn/4-26 # Need the sequencer and the bust record for the MM4005 and XPS trajectory scanning -# Recommended SNCSEQ release: R2.1.16 -SNCSEQ=$(SUPPORT)/seq/seq-2-1-16 -# Recommended BUSY release: R1-6 -BUSY=$(SUPPORT)/busy/R1-6 +# Recommended SNCSEQ release: R2-1-12 +SNCSEQ=$(SUPPORT)/seq/2-1-11 +# Recommended BUSY release: R1-5 +BUSY=$(SUPPORT)/busy/1-4 +CALC=$(SUPPORT)/calc/3-4 -# Recommended EPICS release: R3.14.12.4 +AUTOSAVE=$(SUPPORT)/autosave/R5_4_2 + +ONCRPC=$(SUPPORT)/oncrpc/2 +WAIT=$(SUPPORT)/calc/2-9 +SSCAN=$(SUPPORT)/sscan/2-8 + +# Recommended EPICS release: R3.14.12.1 EPICS_BASE= -include $(TOP)/../configure/EPICS_BASE.$(EPICS_HOST_ARCH) @@ -34,9 +41,11 @@ EPICS_BASE= # /motorExApp. To build examples, the top Makefile, # /Makefile must also be edited. -# Recommended IPAC release: R2-12 -IPAC=$(SUPPORT)/ipac/R2-12 +# Recommended IPAC release: R2-11 +IPAC=$(SUPPORT)/ipac/2-11 # The following is only needed for the motor examples in iocBoot. -#!MOTOR=$(TOP) +MOTOR=$(SUPPORT)/motor/R6-8-1 + +include $(TOP)/../../../ISIS_CONFIG.$(EPICS_HOST_ARCH) diff --git a/iocBoot/iocNoAsyn/Makefile b/iocBoot/iocNoAsyn/Makefile index 19285a0ea..1704a34d3 100644 --- a/iocBoot/iocNoAsyn/Makefile +++ b/iocBoot/iocNoAsyn/Makefile @@ -1,6 +1,6 @@ TOP = ../.. include $(TOP)/configure/CONFIG -ARCH = vxWorks-68040 +ARCH = $(EPICS_HOST_ARCH) #!ARCH = vxWorks-ppc604_long TARGETS = cdCommands TARGETS += envPaths diff --git a/iocBoot/iocWithAsyn/Makefile b/iocBoot/iocWithAsyn/Makefile index 97f7c4253..1d92fe4a0 100644 --- a/iocBoot/iocWithAsyn/Makefile +++ b/iocBoot/iocWithAsyn/Makefile @@ -1,7 +1,6 @@ TOP = ../.. include $(TOP)/configure/CONFIG -ARCH = linux-x86 -#ARCH = win32-x86 +ARCH = $(EPICS_HOST_ARCH) #ARCH = vxWorks-ppc604 #ARCH = windows-x64 TARGETS = cdCommands diff --git a/iocBoot/iocmotorSim/Makefile b/iocBoot/iocmotorSim/Makefile new file mode 100755 index 000000000..6812420ae --- /dev/null +++ b/iocBoot/iocmotorSim/Makefile @@ -0,0 +1,14 @@ +TOP = ../.. +include $(TOP)/configure/CONFIG +ARCH = $(EPICS_HOST_ARCH) +#ARCH = linux-x86_64 +#ARCH = vxWorks-ppc604 +TARGETS = cdCommands +TARGETS += envPaths dllPath.bat runIOC.bat +TARGETS += motor.substitutions.local + +motor.substitutions.local : motor.substitutions + $(MSI) -M "IOC=$(MYPVPREFIX)" -o $@ $< + +include $(TOP)/configure/RULES.ioc + diff --git a/iocBoot/iocmotorSim/README b/iocBoot/iocmotorSim/README new file mode 100644 index 000000000..f7ddad1ad --- /dev/null +++ b/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/iocBoot/iocmotorSim/auto_positions.req b/iocBoot/iocmotorSim/auto_positions.req new file mode 100644 index 000000000..64e804c38 --- /dev/null +++ b/iocBoot/iocmotorSim/auto_positions.req @@ -0,0 +1,8 @@ +$(P)m1.DVAL +$(P)m2.DVAL +$(P)m3.DVAL +$(P)m4.DVAL +$(P)m5.DVAL +$(P)m6.DVAL +$(P)m7.DVAL +$(P)m8.DVAL diff --git a/iocBoot/iocmotorSim/auto_settings.req b/iocBoot/iocmotorSim/auto_settings.req new file mode 100644 index 000000000..64d745c47 --- /dev/null +++ b/iocBoot/iocmotorSim/auto_settings.req @@ -0,0 +1,11 @@ +$(P)saveData_config + +file motor_settings.req P=$(P),M=m1 +file motor_settings.req P=$(P),M=m2 +file motor_settings.req P=$(P),M=m3 +file motor_settings.req P=$(P),M=m4 +file motor_settings.req P=$(P),M=m5 +file motor_settings.req P=$(P),M=m6 +file motor_settings.req P=$(P),M=m7 +file motor_settings.req P=$(P),M=m8 + diff --git a/iocBoot/iocmotorSim/motor.substitutions b/iocBoot/iocmotorSim/motor.substitutions new file mode 100644 index 000000000..44acf3759 --- /dev/null +++ b/iocBoot/iocmotorSim/motor.substitutions @@ -0,0 +1,14 @@ +file "../../db/basic_asyn_motor.db" +{ +pattern +{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT} +{"$(IOC)", 1, "m$(N)", "asynMotor", motorSim1, 0, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} +{"$(IOC)", 2, "m$(N)", "asynMotor", motorSim1, 1, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} +{"$(IOC)", 3, "m$(N)", "asynMotor", motorSim1, 2, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} +{"$(IOC)", 4, "m$(N)", "asynMotor", motorSim1, 3, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} + +{"$(IOC)", 5, "m$(N)", "asynMotor", motorSim2, 0, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} +{"$(IOC)", 6, "m$(N)", "asynMotor", motorSim2, 1, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} +{"$(IOC)", 7, "m$(N)", "asynMotor", motorSim2, 2, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} +{"$(IOC)", 8, "m$(N)", "asynMotor", motorSim2, 3, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} +} diff --git a/iocBoot/iocmotorSim/motor.substitutions.allstop b/iocBoot/iocmotorSim/motor.substitutions.allstop new file mode 100644 index 000000000..a7f28a15a --- /dev/null +++ b/iocBoot/iocmotorSim/motor.substitutions.allstop @@ -0,0 +1,14 @@ +file "../../db/stop_all.db" +{ +pattern +{P, N, M, AS} +{"NDW1298:sjb99183:", 1, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 2, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 3, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 4, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} + +{"NDW1298:sjb99183:", 5, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 6, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 7, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 8, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +} diff --git a/iocBoot/iocmotorSim/saveData.req b/iocBoot/iocmotorSim/saveData.req new file mode 100644 index 000000000..cdb157f90 --- /dev/null +++ b/iocBoot/iocmotorSim/saveData.req @@ -0,0 +1,43 @@ +[prefix] +$(P) + +[status] +$(P)saveData_status + +[message] +$(P)saveData_message + +[filename] +$(P)saveData_fileName + +[counter] # scan counter +$(P)saveData_scanNumber + +[fileSystem] # scan file system +$(P)saveData_fileSystem + +[subdir] # scan file subdirectory +$(P)saveData_subDir + +[basename] # PV containing the desired data-file base name +$(P)saveData_baseName + +[fullPathName] +$(P)saveData_fullPathName + +[realTime1D] # if nonzero, write 1D data as it comes in +$(P)saveData_realTime1D + +[scanRecord] # specify scan records to be monitored +$(P)scanH +$(P)scan1 +$(P)scan2 +$(P)scan3 +$(P)scan4 + +[extraPV] +$(P)userCalc1.CALC "string" +$(P)saveData_realTime1D "enum" +$(P)saveData_scanNumber "long" +$(P)saveData_fileSystem "filesystem" +$(P)scan1.P1SM "scan mode" diff --git a/iocBoot/iocmotorSim/save_restore.cmd b/iocBoot/iocmotorSim/save_restore.cmd new file mode 100644 index 000000000..b6b9e7010 --- /dev/null +++ b/iocBoot/iocmotorSim/save_restore.cmd @@ -0,0 +1,68 @@ + +# BEGIN save_restore.cmd ------------------------------------------------------ + +### save_restore setup +# +# This file does not require modification for standard use, but... + +# status PVs +#save_restoreSet_UseStatusPVs(1) +save_restoreSet_status_prefix("$(MYPVPREFIX)") +dbLoadRecords("$(AUTOSAVE)/asApp/Db/save_restoreStatus.db", "P=$(MYPVPREFIX), DEAD_SECONDS=5") + +# Ok to save/restore save sets with missing values (no CA connection to PV)? +save_restoreSet_IncompleteSetsOk(1) + +# Save dated backup files? +save_restoreSet_DatedBackupFiles(1) + +# Number of sequenced backup files to write +save_restoreSet_NumSeqFiles(3) + +# Time interval between sequenced backups +save_restoreSet_SeqPeriodInSeconds(300) + +# specify where save files should be +set_savefile_path("$(TOP)/iocBoot/$(IOC)", "autosave") + +### +# specify what save files should be restored. Note these files must be +# in the directory specified in set_savefile_path(), or, if that function +# has not been called, from the directory current when iocInit is invoked +set_pass0_restoreFile("auto_positions.sav") +set_pass0_restoreFile("auto_settings.sav") +set_pass1_restoreFile("auto_settings.sav") + +# Note that you can restore a .sav file without also autosaving to it. +#set_pass0_restoreFile("myInitData.sav") +#set_pass1_restoreFile("myInitData.sav") + +### +# specify directories in which to to search for included request files +set_requestfile_path("$(TOP)/iocBoot/$(IOC)", "") +set_requestfile_path("$(TOP)/iocBoot/$(IOC)", "autosave") +set_requestfile_path("$(AREA_DETECTOR)", "ADApp/Db") +set_requestfile_path("$(AUTOSAVE)", "asApp/Db") +set_requestfile_path("$(CALC)", "calcApp/Db") +set_requestfile_path("$(CAMAC)", "camacApp/Db") +set_requestfile_path("$(DAC128V)", "dac128VApp/Db") +set_requestfile_path("$(DXP)", "dxpApp/Db") +set_requestfile_path("$(IP)", "ipApp/Db") +set_requestfile_path("$(IP330)", "ip330App/Db") +set_requestfile_path("$(IPUNIDIG)", "ipUnidigApp/Db") +set_requestfile_path("$(LOVE)", "loveApp/Db") +set_requestfile_path("$(MCA)", "mcaApp/Db") +set_requestfile_path("$(MODBUS)", "modbusApp/Db") +set_requestfile_path("$(MOTOR)", "motorApp/Db") +set_requestfile_path("$(OPTICS)", "opticsApp/Db") +set_requestfile_path("$(QUADEM)", "quadEMApp/Db") +set_requestfile_path("$(SSCAN)", "sscanApp/Db") +set_requestfile_path("$(STD)", "stdApp/Db") +set_requestfile_path("$(VAC)", "vacApp/Db") +set_requestfile_path("$(VME)", "vmeApp/Db") +set_requestfile_path("$(TOP)", "xxxApp/Db") + +# Debug-output level +save_restoreSet_Debug(0) + +# END save_restore.cmd -------------------------------------------------------- diff --git a/iocBoot/iocmotorSim/st.cmd.Vx b/iocBoot/iocmotorSim/st.cmd.Vx new file mode 100644 index 000000000..99b539a85 --- /dev/null +++ b/iocBoot/iocmotorSim/st.cmd.Vx @@ -0,0 +1,27 @@ +# The is the ASYN example for communication to 4 simulated motors +# "#!" marks lines that can be uncommented. + +# The following must be added for many board support packages +#!cd "... IOC st.cmd complete directory path ... " + +< cdCommands +#!< ../nfsCommands + +cd topbin + +# If the VxWorks kernel was built using the project facility, the following must +# be added before any C++ code is loaded (see SPR #28980). +sysCplusEnable=1 + +ld(0,0,"WithAsynVx.munch") + +cd startup +dbLoadDatabase("$(TOP)/dbd/WithAsynVx.dbd") +WithAsynVx_registerRecordDeviceDriver(pdbbase) +dbLoadTemplate("motor.substitutions") + +# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) +motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) +# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) +drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) +iocInit diff --git a/iocBoot/iocmotorSim/st.cmd.allstop b/iocBoot/iocmotorSim/st.cmd.allstop new file mode 100644 index 000000000..a098451ea --- /dev/null +++ b/iocBoot/iocmotorSim/st.cmd.allstop @@ -0,0 +1,56 @@ +# The is the ASYN example for communication to 4 simulated motors +# "#!" marks lines that can be uncommented. +# +# run with ../../bin/${EPICS_HOST_ARCH}/motorSim.exe + +< envPaths + +# save_restore.cmd needs the full path to the startup directory, which +# envPaths currently does not provide +epicsEnvSet(STARTUP,$(TOP)/iocBoot/$(IOC)) +epicsEnvSet "STOP_ALL_DB" "../../db/stop_all.db" + + +#dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") +#WithAsyn_registerRecordDeviceDriver(pdbbase) +dbLoadDatabase("$(TOP)/dbd/motorSim.dbd") +motorSim_registerRecordDeviceDriver(pdbbase) +dbLoadTemplate("motor.substitutions.local") +#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m1,AS=$(MYPVPREFIX)MTR0:Count") +#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m2,AS=$(MYPVPREFIX)MTR0:Count") +#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m3,AS=$(MYPVPREFIX)MTR0:Count") +#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m4,AS=$(MYPVPREFIX)MTR0:Count") +#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m5,AS=$(MYPVPREFIX)MTR0:Count") +#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m6,AS=$(MYPVPREFIX)MTR0:Count") +#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m7,AS=$(MYPVPREFIX)MTR0:Count") +#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m8,AS=$(MYPVPREFIX)MTR0:Count") +dbLoadTemplate("motor.substitutions.allstop") + +### save_restore setup +# We presume a suitable initHook routine was compiled into xxx.munch. +# See also create_monitor_set(), after iocInit() . +#< save_restore.cmd + +# 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 + +# save positions every five seconds +#create_monitor_set("auto_positions.req",5,"P=$(MYPVPREFIX)") +# save other things every thirty seconds +#create_monitor_set("auto_settings.req",30,"P=$(MYPVPREFIX)") + +### Start the saveData task. +#saveData_Init("saveData.req", "P=$(MYPVPREFIX)") diff --git a/iocBoot/iocmotorSim/st.cmd.motorsim b/iocBoot/iocmotorSim/st.cmd.motorsim new file mode 100644 index 000000000..517f1e6e8 --- /dev/null +++ b/iocBoot/iocmotorSim/st.cmd.motorsim @@ -0,0 +1,49 @@ +# The is the ASYN example for communication to 4 simulated motors +# "#!" marks lines that can be uncommented. +# +# run with ../../bin/${EPICS_HOST_ARCH}/motorSim.exe + +< envPaths + +# save_restore.cmd needs the full path to the startup directory, which +# envPaths currently does not provide +epicsEnvSet(STARTUP,$(TOP)/iocBoot/$(IOC)) + +dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") +WithAsyn_registerRecordDeviceDriver(pdbbase) + +##dbLoadDatabase("$(TOP)/dbd/motorSim.dbd") +##motorSim_registerRecordDeviceDriver(pdbbase) + +##dbLoadTemplate("motor.substitutions.local") +dbLoadRecords("$(TOP)/db/motorSimTest.db") + + +### save_restore setup +# We presume a suitable initHook routine was compiled into xxx.munch. +# See also create_monitor_set(), after iocInit() . +#< save_restore.cmd + +# 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 + +# save positions every five seconds +#create_monitor_set("auto_positions.req",5,"P=$(MYPVPREFIX)") +# save other things every thirty seconds +#create_monitor_set("auto_settings.req",30,"P=$(MYPVPREFIX)") + +### Start the saveData task. +#saveData_Init("saveData.req", "P=$(MYPVPREFIX)") diff --git a/iocBoot/iocmotorSim/st.cmd.sim.inst b/iocBoot/iocmotorSim/st.cmd.sim.inst new file mode 100644 index 000000000..cd9d40c03 --- /dev/null +++ b/iocBoot/iocmotorSim/st.cmd.sim.inst @@ -0,0 +1,28 @@ +# The is the ASYN example for communication to 4 simulated motors +# "#!" marks lines that can be uncommented. +# +# run with ../../bin/${EPICS_HOST_ARCH}/motorSim.exe + +< envPaths + +#dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") +#WithAsyn_registerRecordDeviceDriver(pdbbase) +dbLoadDatabase("$(TOP)/dbd/motorSim.dbd") +motorSim_registerRecordDeviceDriver(pdbbase) +dbLoadTemplate("$(SETTINGS)/motor.substitutions.local") + +# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) +motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) +# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) +drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) + +motorSimCreateController("motorSim2", 8) +#asynSetTraceIOMask("motorSim2", 0, 4) +#asynSetTraceMask("motorSim2", 0, 255) + +# motorSimConfigAxis(port, axis, lowLimit, highLimit, home, start) +motorSimConfigAxis("motorSim2", 0, 20000, -20000, 500, 0) +motorSimConfigAxis("motorSim2", 1, 20000, -20000, 1500, 0) +motorSimConfigAxis("motorSim2", 2, 20000, -20000, 2500, 0) +motorSimConfigAxis("motorSim2", 3, 20000, -20000, 3000, 0) +iocInit diff --git a/iocBoot/iocmotorSim/st.cmd.unix b/iocBoot/iocmotorSim/st.cmd.unix new file mode 100644 index 000000000..7d693df62 --- /dev/null +++ b/iocBoot/iocmotorSim/st.cmd.unix @@ -0,0 +1,27 @@ +# The is the ASYN example for communication to 4 simulated motors +# "#!" marks lines that can be uncommented. +# +# run with ../../bin/${EPICS_HOST_ARCH}/WithASYN.exe + +< envPaths + +dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") +WithAsyn_registerRecordDeviceDriver(pdbbase) + +dbLoadTemplate("motor.substitutions.local") + +# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) +motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) +# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) +drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) + +motorSimCreateController("motorSim2", 8) +#asynSetTraceIOMask("motorSim2", 0, 4) +#asynSetTraceMask("motorSim2", 0, 255) + +# motorSimConfigAxis(port, axis, lowLimit, highLimit, home, start) +motorSimConfigAxis("motorSim2", 0, 20000, -20000, 500, 0) +motorSimConfigAxis("motorSim2", 1, 20000, -20000, 1500, 0) +motorSimConfigAxis("motorSim2", 2, 20000, -20000, 2500, 0) +motorSimConfigAxis("motorSim2", 3, 20000, -20000, 3000, 0) +iocInit diff --git a/iocBoot/iocmotorSim/st.cmd.win32 b/iocBoot/iocmotorSim/st.cmd.win32 new file mode 100755 index 000000000..7f861f0c9 --- /dev/null +++ b/iocBoot/iocmotorSim/st.cmd.win32 @@ -0,0 +1,14 @@ +# The is the ASYN example for communication to 4 simulated motors +# "#!" marks lines that can be uncommented. + +< envPaths + +dbLoadDatabase("$(TOP)/dbd/WithMPFWin32.dbd") +WithMPFWin32_registerRecordDeviceDriver(pdbbase) +dbLoadTemplate("motor.substitutions") + +# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) +motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) +# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) +drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) +iocInit diff --git a/iocBoot/iocmotorSim/start_medm b/iocBoot/iocmotorSim/start_medm new file mode 100755 index 000000000..9cd0cba70 --- /dev/null +++ b/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/motorApp/Db/EnsemblePSOFly.db b/motorApp/Db/EnsemblePSOFly.db index ed9b63600..be9cf395c 100644 --- a/motorApp/Db/EnsemblePSOFly.db +++ b/motorApp/Db/EnsemblePSOFly.db @@ -24,7 +24,7 @@ # PSOCONTROL X OFF record(ao, "$(P)$(Q)startPos") { field(DESC, "data aqc start") - field(EGU, "degrees") + field(EGU, "degree") field(PREC, "5") } @@ -38,7 +38,7 @@ record(calcout, "$(P)$(Q)startPosDial") { record(ao, "$(P)$(Q)endPos") { field(DESC, "data aqc end") - field(EGU, "degrees") + field(EGU, "degree") field(PREC, "5") } @@ -51,7 +51,7 @@ record(calcout, "$(P)$(Q)endPosDial") { } record(ao, "$(P)$(Q)slewSpeed") { - field(EGU, "degrees/s") + field(EGU, "degree/s") field(PREC, "5") } @@ -109,7 +109,7 @@ record(ao, "$(P)$(Q)scanDelta") { field(DRVH, "180") field(DRVL, ".0001") field(DESC, "data aqc end") - field(EGU, "degrees") + field(EGU, "degree") field(PREC, "5") field(OUT, "$(P)$(Q)checkScanDelta.A PP") } diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index f872ed32b..79332afac 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -18,6 +18,7 @@ include $(TOP)/configure/CONFIG DB += motor.db DB += basic_motor.db DB += basic_asyn_motor.db +DB += stop_all.db DB += IMS_extra.db DB += XPSAuxAi.db DB += XPSAuxAo.db diff --git a/motorApp/Db/SoftMotorEx.db b/motorApp/Db/SoftMotorEx.db index ff015bb14..aa10d82fe 100644 --- a/motorApp/Db/SoftMotorEx.db +++ b/motorApp/Db/SoftMotorEx.db @@ -30,7 +30,7 @@ record(motor, "$(user):linear") { field(OUT, "#C0 S0 @") field(MRES, "0.001") field(PREC, "3") - field(EGU, "mm.") + field(EGU, "mm") field(DHLM, "1000") field(DLLM, "-1000") field(RTRY, "0") @@ -46,7 +46,7 @@ record(motor, "$(user):rotary") { field(RRES, "1.0") field(URIP, "Yes") field(PREC, "3") - field(EGU, "deg.") + field(EGU, "degree") field(DHLM, "45") field(DLLM, "-45") field(RTRY, "0") diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index ef5a7d5a0..d55293a11 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -11,13 +11,47 @@ record(bo, "$(P)allstop") { field(DESC, "Stop All IOC motors.") } +# Monitors the global STOP:ALL PV. When this PV changes, '1' is +# sent to $(P)allstop over channel access. +record(calcout, "$(P)allstop:MONITOR") { + field(SCAN, ".1 second") + field(DESC, "Stop All IOC motors.") + field(INPA, "$(PVPREFIX)CS:MOT:STOP:ALL PP") + field(OOPT, "On Change") + field(CALC, "A") + field(DOPT, "Use OCAL") + field(OUT, "$(P)allstop.VAL CA") + field(OCAL, 1) +} + # Indicates if all motors in this IOC are done moving. # Set by motorUtil when $(P)moving.VAL == 0. -record(bi, "$(P)alldone") { +#record(bi, "$(P)alldone") { +# note: have changed from "bi" to "bo" so can push output value properly +record(bo, "$(P)alldone") { field(ZNAM, "moving") field(ONAM, "done") - field(INP, "1") + field(VAL, "1") field(DESC, "All IOC motors done moving.") + field(OUT, "$(P)_MOVING.A PP") +} + +record(calcout, "$(P)_MOVING") +{ + field(SCAN, "Passive") + field(CALC, "A") + field(VAL, "1") + field(OOPT, "On Change") + field(DOPT, "Use OCAL") + field(OCAL, "(1-2*A)") + field(OUT, "$(P)_SEQ.DO1 PP") +} + +# need to use an SSEQ with WAIT to make sure we do not loose a PUT +record(sseq, "$(P)_SEQ") +{ + field(LNK1, "$(PVPREFIX)CS:MOT:MOVING:SP CA") + field(WAIT1, "Wait") } # The number of motors moving in this IOC. @@ -27,12 +61,12 @@ record(longout, "$(P)moving") { } # Used by xxx.adl to blink "Moving" indicator. -record(calc, "$(P)alldoneBlink") { - field(SCAN, ".5 second") - field(CALC, "A?0:!B") - field(INPA, "$(P)alldone") - field(INPB, "$(P)alldoneBlink") -} +#record(calc, "$(P)alldoneBlink") { +# field(SCAN, ".5 second") +# field(CALC, "A?0:!B") +# field(INPA, "$(P)alldone") +# field(INPB, "$(P)alldoneBlink") +#} # Allow smart clients to maintain a list of moving motors record(waveform, "$(P)movingDiff") { diff --git a/motorApp/Db/motor_positions.req b/motorApp/Db/motor_positions.req index 2d8666bb3..731a90302 100644 --- a/motorApp/Db/motor_positions.req +++ b/motorApp/Db/motor_positions.req @@ -1,2 +1,2 @@ # FILE... motor_positions.req -$(P)$(M).DVAL +#$(P)$(M).DVAL diff --git a/motorApp/Db/motor_settings.req b/motorApp/Db/motor_settings.req index 57a0d9be6..f8fb93776 100644 --- a/motorApp/Db/motor_settings.req +++ b/motorApp/Db/motor_settings.req @@ -5,4 +5,6 @@ # HeadURL: $URL$ file basic_motor_settings.req P=$(P),M=$(M) +$(P)$(M).RMOD $(P)$(M)_able.VAL +$(P)$(M).SPMG diff --git a/motorApp/Db/stop_all.db b/motorApp/Db/stop_all.db new file mode 100644 index 000000000..ffb0036df --- /dev/null +++ b/motorApp/Db/stop_all.db @@ -0,0 +1,11 @@ +record(calcout, "$(P)$(M):ALLSTOP") +{ + field(SCAN, ".1 second") + field(INPA, "$(AS):STOP:ALL") + field(INPB, "$(P)$(M).DMOV") + field(CALC, "A") + field(OUT, "$(P)$(M).STOP") + field(OCAL, "(1-B)*A") + field(DOPT, "Use OCAL") + field(OOPT, "On Change") +} diff --git a/motorApp/Db/stopping_motor.db b/motorApp/Db/stopping_motor.db new file mode 100644 index 000000000..675962c55 --- /dev/null +++ b/motorApp/Db/stopping_motor.db @@ -0,0 +1,31 @@ +record(swait, "$(P)$(M):ALLSTOP") +{ + field(SCAN, "1 second") + field(INAN, "$(AS)") + field(INAP, "Yes") + field(CALC, "A") + field(OUTN, "$(P)$(M).STOP") + field(DOPT, "Use VAL") + field(OOPT, "On Change") +} +record(motor,"$(P)$(M)") +{ + field(DESC,"$(DESC)") + field(DTYP,"$(DTYP)") + field(DIR,"$(DIR)") + field(VELO,"$(VELO)") + field(VBAS,"$(VBAS)") + field(ACCL,"$(ACCL)") + field(BDST,"$(BDST)") + field(BVEL,"$(BVEL)") + field(BACC,"$(BACC)") + field(OUT,"@asyn($(PORT),$(ADDR))") + field(MRES,"$(MRES)") + field(PREC,"$(PREC)") + field(EGU,"$(EGU)") + field(DHLM,"$(DHLM)") + field(DLLM,"$(DLLM)") + field(INIT,"$(INIT)") + field(TWV,"1") +} + diff --git a/motorApp/MotorSimSrc/Makefile b/motorApp/MotorSimSrc/Makefile index cfde6f152..ff6eabb50 100644 --- a/motorApp/MotorSimSrc/Makefile +++ b/motorApp/MotorSimSrc/Makefile @@ -37,6 +37,8 @@ motorSim_DBD += base.dbd motorSim_DBD += asyn.dbd motorSim_DBD += motorSupport.dbd motorSim_DBD += motorSimSupport.dbd +motorSim_DBD += asSupport.dbd +motorSim_DBD += calcSupport.dbd # _registerRecordDeviceDriver.cpp will be created from .dbd motorSim_SRCS += motorSim_registerRecordDeviceDriver.cpp @@ -49,6 +51,8 @@ motorSim_OBJS_vxWorks += $(EPICS_BASE_BIN)/vxComLibrary motorSim_LIBS += motorSimSupport motorSim_LIBS += motor motorSim_LIBS += asyn +motorSim_LIBS += autosave +motorSim_LIBS += calc sscan motorSim_LIBS += $(EPICS_BASE_IOC_LIBS) diff --git a/motorApp/MotorSrc/devMotorAsyn.c b/motorApp/MotorSrc/devMotorAsyn.c index 00d6ef887..49c2989fc 100644 --- a/motorApp/MotorSrc/devMotorAsyn.c +++ b/motorApp/MotorSrc/devMotorAsyn.c @@ -602,7 +602,7 @@ static RTN_STATUS build_trans( motor_cmnd command, /* Queue asyn request, so we get a callback when driver is ready */ pasynUser->reason = pPvt->driverReasons[pmsg->command]; - status = pasynManager->queueRequest(pasynUser, 0, 0); + status = pasynManager->queueRequest(pasynUser, asynQueuePriorityHigh, 0); if (status != asynSuccess) { asynPrint(pasynUser, ASYN_TRACE_ERROR, "devMotorAsyn::build_trans: %s error calling queueRequest, %s\n", diff --git a/motorApp/MotorSrc/motorUtil.cc b/motorApp/MotorSrc/motorUtil.cc index 41f8669b2..a793a625e 100644 --- a/motorApp/MotorSrc/motorUtil.cc +++ b/motorApp/MotorSrc/motorUtil.cc @@ -72,7 +72,7 @@ typedef struct motor_pv_info /* ----- Global Variables ----- */ -int motorUtil_debug = 0; +volatile int motorUtil_debug = 0; int numMotors = 0; /* ----- ---------------- ----- */ @@ -113,6 +113,7 @@ RTN_STATUS motorUtilInit(char *vme_name) return(status); } +static CA_SYNC_GID moving_sync_gid; static int motorUtil_task(void *arg) { @@ -183,6 +184,9 @@ static int motorUtil_task(void *arg) } } + SEVCHK ( ca_sg_create ( &moving_sync_gid ), "motorUtil: Sync group create failed" ); + + /* Wait on a (never signalled) event here, rather than suspending the thread, so as not to show up in the thread list as "SUSPENDED", which is usually a sign of a fault. */ @@ -323,15 +327,16 @@ static void moving(int callback_motor_index, chid callback_chid, if (motorUtil_debug) errlogPrintf("sending alldone = TRUE\n"); - ca_put(DBR_SHORT, chid_alldone, &done); + ca_sg_array_put(moving_sync_gid, DBR_SHORT, 1, chid_alldone, &done); +// ca_put(DBR_SHORT, chid_alldone, &done); old_alldone_value = new_alldone_value; } else { if (motorUtil_debug) errlogPrintf("sending alldone = FALSE\n"); - - ca_put(DBR_SHORT, chid_alldone, ¬_done); + ca_sg_array_put(moving_sync_gid, DBR_SHORT, 1, chid_alldone, ¬_done); +// ca_put(DBR_SHORT, chid_alldone, ¬_done); old_alldone_value = new_alldone_value; } } @@ -345,7 +350,8 @@ static void moving(int callback_motor_index, chid callback_chid, errlogPrintf("updating number of motors moving\n"); /* give $(P)moving the appropriate value */ - ca_put(DBR_LONG, chid_moving, &numMotorsMoving); + ca_sg_array_put(moving_sync_gid, DBR_LONG, 1, chid_moving, &numMotorsMoving); +// ca_put(DBR_LONG, chid_moving, &numMotorsMoving); /* Tell which motor's dmov changed */ sprintf(diffStr, "%c%s", diffChar, motorArray[callback_motor_index].name); @@ -358,6 +364,7 @@ static void moving(int callback_motor_index, chid callback_chid, /* send the ca_puts */ status = ca_flush_io(); + status = ca_sg_block(moving_sync_gid, 1.0); } diff --git a/motorApp/PIGCS2Src/PIasynController.cpp b/motorApp/PIGCS2Src/PIasynController.cpp index a6278763f..67602c197 100644 --- a/motorApp/PIGCS2Src/PIasynController.cpp +++ b/motorApp/PIGCS2Src/PIasynController.cpp @@ -44,6 +44,7 @@ Based on drvMotorSim.c, Mark Rivers, December 13, 2009 #include "PIInterface.h" #include +#include //#undef asynPrint //#define asynPrint(user,reason,format...) 0 diff --git a/motorApp/PhytronSrc/phytronAxisMotor.cpp b/motorApp/PhytronSrc/phytronAxisMotor.cpp index 771495378..db9f54ed6 100644 --- a/motorApp/PhytronSrc/phytronAxisMotor.cpp +++ b/motorApp/PhytronSrc/phytronAxisMotor.cpp @@ -19,7 +19,9 @@ HeadURL: $URL$ #include #include #include +#ifndef _WIN32 #include +#endif /* _WIN32 */ #include #include diff --git a/motorExApp/WithAsyn/Makefile b/motorExApp/WithAsyn/Makefile index 70ebb7d64..58b1ba82a 100644 --- a/motorExApp/WithAsyn/Makefile +++ b/motorExApp/WithAsyn/Makefile @@ -84,10 +84,11 @@ COMMONLIBS += motor # Needed for Newport SNL programs WithAsyn_LIBS += $(COMMONLIBS) -WithAsyn_LIBS += asyn ifdef SNCSEQ WithAsyn_LIBS += seqDev seq pv endif +WithAsyn_LIBS += busy +WithAsyn_LIBS += asyn WithAsyn_LIBS += $(EPICS_BASE_IOC_LIBS) WithAsynVx_LIBS += $(COMMONLIBS) From 83cbca2c63ddc6ffc4c05548b31f8de56e1ffb48 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 20 Feb 2015 17:04:53 +0000 Subject: [PATCH 003/232] Allow ms as unit and fix other motor record units Former-commit-id: 5aadf6f4a7bb747ecfc44137614277d65b7741dc --- motorApp/Db/Phytron_I1AM01.db | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/motorApp/Db/Phytron_I1AM01.db b/motorApp/Db/Phytron_I1AM01.db index 01e1f1663..df9d890c0 100644 --- a/motorApp/Db/Phytron_I1AM01.db +++ b/motorApp/Db/Phytron_I1AM01.db @@ -758,7 +758,7 @@ record(ao, "$(P)$(M)-ENC-RES_SET") field(DESC, "Encoder resolution") field(DTYP, "asynInt32") field(OUT, "@asyn($(PORT), $(ADDR), $(TIMEOUT))ENC_RESOLUTION") - field(EGU, "b") + field(EGU, "bit") field(HOPR, "48") field(LOPR, "0") field(FLNK, "$(P)$(M)-ENC-RES_GET") @@ -769,7 +769,7 @@ record(ai, "$(P)$(M)-ENC-RES_GET") field(DESC, "Encoder resolution") field(DTYP, "asynInt32") field(INP, "@asyn($(PORT), $(ADDR), $(TIMEOUT))ENC_RESOLUTION") - field(EGU, "b") + field(EGU, "bit") field(HOPR, "48") field(LOPR, "0") field(PINI, "YES") @@ -957,7 +957,7 @@ record(ao, "$(P)$(M)-CURRENT-DELAY_SET") field(DESC, "Current hold time") field(DTYP, "asynInt32") field(OUT, "@asyn($(PORT), $(ADDR), $(TIMEOUT))CURRENT_DELAY_TIME") - field(EGU, "msec") + field(EGU, "ms") field(FLNK, "$(P)$(M)-CURRENT-DELAY_GET") } @@ -1047,7 +1047,7 @@ record(ai, "$(P)$(M)-PS-TEMPERATURE") field(DESC, "Power stage temp") field(DTYP, "asynFloat64") field(INP, "@asyn($(PORT), $(ADDR), $(TIMEOUT))PS_TEMPERATURE") - field(EGU, "°C") + field(EGU, "C") field(SCAN, "$(SCAN)") field(PREC, 1) field(PINI, "YES") @@ -1085,7 +1085,7 @@ record(ai, "$(P)$(M)-MOTOR-TEMP") field(DTYP, "asynFloat64") field(INP, "@asyn($(PORT), $(ADDR), $(TIMEOUT))MOTOR_TEMP") field(SCAN, "$(SCAN)") - field(EGU, "°C") + field(EGU, "C") field(PREC, 1) field(FLNK, "$(P)$(M)-MOTOR-TEMP") } From 91ef5437fd8f518b5f668004531054c1fbbb170e Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 20 Feb 2015 17:07:19 +0000 Subject: [PATCH 004/232] Update to motor 6-9 Former-commit-id: 933ed4c02b301b009bf50b63c09b6d93a27cb354 --- configure/RELEASE | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configure/RELEASE b/configure/RELEASE index 17aa191e4..0c60f5531 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -45,7 +45,7 @@ EPICS_BASE= IPAC=$(SUPPORT)/ipac/2-11 # The following is only needed for the motor examples in iocBoot. -MOTOR=$(SUPPORT)/motor/R6-8-1 +MOTOR=$(SUPPORT)/motor/R6-9 include $(TOP)/../../../ISIS_CONFIG.$(EPICS_HOST_ARCH) From b709adec0735415a15ea24e0e439205404dc667d Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sat, 21 Feb 2015 17:15:08 +0000 Subject: [PATCH 005/232] Fix export include order Former-commit-id: c34ee93aa0c061eefe851471b481969dd0341355 --- motorApp/NewportSrc/SMC100Driver.cpp | 3 +++ motorApp/NewportSrc/SMC100Driver.h | 3 --- motorApp/NewportSrc/XPSAxis.h | 3 --- motorApp/NewportSrc/XPSController.cpp | 6 +++--- 4 files changed, 6 insertions(+), 9 deletions(-) diff --git a/motorApp/NewportSrc/SMC100Driver.cpp b/motorApp/NewportSrc/SMC100Driver.cpp index 4fdcb22c9..b7793807e 100644 --- a/motorApp/NewportSrc/SMC100Driver.cpp +++ b/motorApp/NewportSrc/SMC100Driver.cpp @@ -23,6 +23,9 @@ K. Goetze 2012-03-23 Initial version #include +#include "asynMotorController.h" +#include "asynMotorAxis.h" + #include #include "SMC100Driver.h" diff --git a/motorApp/NewportSrc/SMC100Driver.h b/motorApp/NewportSrc/SMC100Driver.h index ddd39b847..cb3264ec2 100644 --- a/motorApp/NewportSrc/SMC100Driver.h +++ b/motorApp/NewportSrc/SMC100Driver.h @@ -10,9 +10,6 @@ K. Goetze 2012-03-23 */ -#include "asynMotorController.h" -#include "asynMotorAxis.h" - #define MAX_SMC100_AXES 1 // No controller-specific parameters yet diff --git a/motorApp/NewportSrc/XPSAxis.h b/motorApp/NewportSrc/XPSAxis.h index 44a684f50..1bc4eac33 100644 --- a/motorApp/NewportSrc/XPSAxis.h +++ b/motorApp/NewportSrc/XPSAxis.h @@ -5,9 +5,6 @@ USAGE... Newport XPS EPICS asyn motor device driver #ifndef XPSMotorAxis_H #define XPSMotorAxis_H -#include "asynMotorController.h" -#include "asynMotorAxis.h" - /** Struct that contains information about the XPS corrector loop.*/ typedef struct { diff --git a/motorApp/NewportSrc/XPSController.cpp b/motorApp/NewportSrc/XPSController.cpp index 9e67142dd..8e37ebafe 100644 --- a/motorApp/NewportSrc/XPSController.cpp +++ b/motorApp/NewportSrc/XPSController.cpp @@ -91,13 +91,13 @@ Versions: Release 4-5 and higher. #include #include -#include "XPS_C8_drivers.h" -#include "xps_ftp.h" #include "asynMotorController.h" #include "asynMotorAxis.h" -#include "XPSAxis.h" #include +#include "XPS_C8_drivers.h" +#include "xps_ftp.h" +#include "XPSAxis.h" #include "XPSController.h" static const char *driverName = "XPSController"; From 819777bf9e09388101b5cf4b1eaa7ee450fcd909 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 23 Feb 2015 11:29:21 +0000 Subject: [PATCH 006/232] Use busy 1-6-1 Former-commit-id: 190fb29ff151eece2a925667eee80bc14f6107b8 --- configure/RELEASE | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configure/RELEASE b/configure/RELEASE index 0c60f5531..e01edf4a7 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -21,7 +21,7 @@ ASYN=$(SUPPORT)/asyn/4-26 # Recommended SNCSEQ release: R2-1-12 SNCSEQ=$(SUPPORT)/seq/2-1-11 # Recommended BUSY release: R1-5 -BUSY=$(SUPPORT)/busy/1-4 +BUSY=$(SUPPORT)/busy/1-6-1 CALC=$(SUPPORT)/calc/3-4 AUTOSAVE=$(SUPPORT)/autosave/R5_4_2 From 5f7330a624bbb034c641de78ecea6872423985fc Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 2 Mar 2015 15:13:14 +0000 Subject: [PATCH 007/232] Remove race condition in stop_all Former-commit-id: c12f4d0aeb347df43eed69e0239ba31e016223ca --- motorApp/Db/stop_all.db | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/motorApp/Db/stop_all.db b/motorApp/Db/stop_all.db index ffb0036df..613223976 100644 --- a/motorApp/Db/stop_all.db +++ b/motorApp/Db/stop_all.db @@ -1,11 +1,10 @@ record(calcout, "$(P)$(M):ALLSTOP") { - field(SCAN, ".1 second") - field(INPA, "$(AS):STOP:ALL") + field(INPA, "$(AS):STOP:ALL CP") field(INPB, "$(P)$(M).DMOV") field(CALC, "A") field(OUT, "$(P)$(M).STOP") field(OCAL, "(1-B)*A") field(DOPT, "Use OCAL") - field(OOPT, "On Change") + field(OOPT, "When Non-zero") } From c4cfe13b3c9d1ad04a3ea01d4e1a468ca530b892 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 5 Mar 2015 09:46:47 +0000 Subject: [PATCH 008/232] make stop all trigger on every write not just change Former-commit-id: 32f6b11f9a88fffd838069babfb2e51486c258a6 --- motorApp/Db/Makefile | 1 - motorApp/Db/motorUtil.db | 8 ++++---- motorApp/Db/stop_all.db | 10 ---------- 3 files changed, 4 insertions(+), 15 deletions(-) delete mode 100644 motorApp/Db/stop_all.db diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index 79332afac..f872ed32b 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -18,7 +18,6 @@ include $(TOP)/configure/CONFIG DB += motor.db DB += basic_motor.db DB += basic_asyn_motor.db -DB += stop_all.db DB += IMS_extra.db DB += XPSAuxAi.db DB += XPSAuxAo.db diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index d55293a11..828930747 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -11,17 +11,17 @@ record(bo, "$(P)allstop") { field(DESC, "Stop All IOC motors.") } + # Monitors the global STOP:ALL PV. When this PV changes, '1' is # sent to $(P)allstop over channel access. record(calcout, "$(P)allstop:MONITOR") { - field(SCAN, ".1 second") field(DESC, "Stop All IOC motors.") - field(INPA, "$(PVPREFIX)CS:MOT:STOP:ALL PP") - field(OOPT, "On Change") + field(INPA, "$(PVPREFIX)CS:MOT:STOP:ALL CP") + field(OOPT, "When Non-zero") field(CALC, "A") field(DOPT, "Use OCAL") - field(OUT, "$(P)allstop.VAL CA") field(OCAL, 1) + field(OUT, "$(P)allstop.VAL CA") } # Indicates if all motors in this IOC are done moving. diff --git a/motorApp/Db/stop_all.db b/motorApp/Db/stop_all.db deleted file mode 100644 index 613223976..000000000 --- a/motorApp/Db/stop_all.db +++ /dev/null @@ -1,10 +0,0 @@ -record(calcout, "$(P)$(M):ALLSTOP") -{ - field(INPA, "$(AS):STOP:ALL CP") - field(INPB, "$(P)$(M).DMOV") - field(CALC, "A") - field(OUT, "$(P)$(M).STOP") - field(OCAL, "(1-B)*A") - field(DOPT, "Use OCAL") - field(OOPT, "When Non-zero") -} From 03aed4069cd04c9fcb1165757fb88419584196f4 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 23 Mar 2015 13:29:05 +0000 Subject: [PATCH 009/232] Add ADEL and MDEL Former-commit-id: 079eafc4566ed3d56572f499ad3e07e7d61e5ac4 --- motorApp/Db/basic_motor_settings.req | 2 ++ 1 file changed, 2 insertions(+) diff --git a/motorApp/Db/basic_motor_settings.req b/motorApp/Db/basic_motor_settings.req index 529046f7d..d31669ad9 100644 --- a/motorApp/Db/basic_motor_settings.req +++ b/motorApp/Db/basic_motor_settings.req @@ -47,4 +47,6 @@ $(P)$(M).PREM $(P)$(M).POST $(P)$(M).FLNK $(P)$(M).RMOD +$(P)$(M).ADEL +$(P)$(M).MDEL From b39de46b896ee298d45ac54a3f80745e4c5f7243 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 24 Mar 2015 11:00:08 +0000 Subject: [PATCH 010/232] Update autosave to 5_6_1 Former-commit-id: da4c98ed321a2aca2d93c5b2b61e8b38502de09f --- configure/RELEASE | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configure/RELEASE b/configure/RELEASE index e01edf4a7..fb2c8479b 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -24,7 +24,7 @@ SNCSEQ=$(SUPPORT)/seq/2-1-11 BUSY=$(SUPPORT)/busy/1-6-1 CALC=$(SUPPORT)/calc/3-4 -AUTOSAVE=$(SUPPORT)/autosave/R5_4_2 +AUTOSAVE=$(SUPPORT)/autosave/R5_6_1 ONCRPC=$(SUPPORT)/oncrpc/2 WAIT=$(SUPPORT)/calc/2-9 From 8eec933e4dd221b7cbe2fddff3379e67d35536b4 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 20 Apr 2015 14:46:49 +0000 Subject: [PATCH 011/232] Change way moving motors is calculated so less likely to miss something Former-commit-id: ca34582f697ca86abf12e1399c45d7c2188a8571 --- motorApp/Db/motorUtil.db | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 828930747..997e70101 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -43,15 +43,21 @@ record(calcout, "$(P)_MOVING") field(VAL, "1") field(OOPT, "On Change") field(DOPT, "Use OCAL") - field(OCAL, "(1-2*A)") - field(OUT, "$(P)_SEQ.DO1 PP") + field(OCAL, "A = 0 ? 1 : 0") + field(OUT, "$(P)_FAN PP") } -# need to use an SSEQ with WAIT to make sure we do not loose a PUT -record(sseq, "$(P)_SEQ") +record(dfanout, "$(P)_FAN") { - field(LNK1, "$(PVPREFIX)CS:MOT:MOVING:SP CA") - field(WAIT1, "Wait") + field(OMSL, "supervisory") + field(SELM, "All") +$(IFDMC01) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.A PP") +$(IFDMC02) field(OUTB, "$(PVPREFIX)CS:MOT:MOVING.B PP") +$(IFDMC03) field(OUTC, "$(PVPREFIX)CS:MOT:MOVING.C PP") +$(IFDMC04) field(OUTD, "$(PVPREFIX)CS:MOT:MOVING.D PP") +$(IFDMC05) field(OUTE, "$(PVPREFIX)CS:MOT:MOVING.E PP") +$(IFDMC06) field(OUTF, "$(PVPREFIX)CS:MOT:MOVING.F PP") +$(IFDMC07) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G PP") } # The number of motors moving in this IOC. From fa548d6d50cc2f0e036d37b27b29317d88710ef9 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 21 Apr 2015 16:05:58 +0000 Subject: [PATCH 012/232] Add periodic processing just in case Former-commit-id: fee3ea727fc2e73641fb37508a67dbaecf116309 --- motorApp/Db/motorUtil.db | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 997e70101..4f2ceaacc 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -26,19 +26,17 @@ record(calcout, "$(P)allstop:MONITOR") { # Indicates if all motors in this IOC are done moving. # Set by motorUtil when $(P)moving.VAL == 0. -#record(bi, "$(P)alldone") { -# note: have changed from "bi" to "bo" so can push output value properly -record(bo, "$(P)alldone") { +record(bi, "$(P)alldone") { field(ZNAM, "moving") field(ONAM, "done") field(VAL, "1") field(DESC, "All IOC motors done moving.") - field(OUT, "$(P)_MOVING.A PP") } record(calcout, "$(P)_MOVING") { field(SCAN, "Passive") + field(INPA, "$(P)alldone CP") field(CALC, "A") field(VAL, "1") field(OOPT, "On Change") @@ -60,6 +58,15 @@ $(IFDMC06) field(OUTF, "$(PVPREFIX)CS:MOT:MOVING.F PP") $(IFDMC07) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G PP") } +# force periodoc processing to catch up any missing, shouldn't +# be necessaryu but no harm to be careful +record(fanout, "$(P)_FAN2") +{ + field(SCAN, "10 second") + field(SELM, "All") + field(LNK1, "$(P)_MOVING.PROC") +} + # The number of motors moving in this IOC. # $(P)moving.VAL is set by motorUtil. record(longout, "$(P)moving") { From c712144dd24eff255d4308c9a9b8bff3a3ca4222 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 22 Apr 2015 18:23:44 +0000 Subject: [PATCH 013/232] Process alldone every time Former-commit-id: c13dea51de2fc9020070e4668f950ea96db0f9b7 --- motorApp/Db/motorUtil.db | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 4f2ceaacc..e64e7d232 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -33,13 +33,14 @@ record(bi, "$(P)alldone") { field(DESC, "All IOC motors done moving.") } +# process "Every Time" so value can be re-applied by periodic processing by _FAN2 record(calcout, "$(P)_MOVING") { field(SCAN, "Passive") field(INPA, "$(P)alldone CP") field(CALC, "A") field(VAL, "1") - field(OOPT, "On Change") + field(OOPT, "Every Time") field(DOPT, "Use OCAL") field(OCAL, "A = 0 ? 1 : 0") field(OUT, "$(P)_FAN PP") From 9d4f8c84b3db8ca2164a5adbcd46c32d699376c2 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 23 Apr 2015 15:10:30 +0000 Subject: [PATCH 014/232] Revert to original - we don't need SG support now, and maybe it causes an extra bounce on alldone ? Former-commit-id: 7f5001e054dabd6a5ce71b992be342b45ba8de69 --- motorApp/MotorSrc/motorUtil.cc | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/motorApp/MotorSrc/motorUtil.cc b/motorApp/MotorSrc/motorUtil.cc index a793a625e..41f8669b2 100644 --- a/motorApp/MotorSrc/motorUtil.cc +++ b/motorApp/MotorSrc/motorUtil.cc @@ -72,7 +72,7 @@ typedef struct motor_pv_info /* ----- Global Variables ----- */ -volatile int motorUtil_debug = 0; +int motorUtil_debug = 0; int numMotors = 0; /* ----- ---------------- ----- */ @@ -113,7 +113,6 @@ RTN_STATUS motorUtilInit(char *vme_name) return(status); } -static CA_SYNC_GID moving_sync_gid; static int motorUtil_task(void *arg) { @@ -184,9 +183,6 @@ static int motorUtil_task(void *arg) } } - SEVCHK ( ca_sg_create ( &moving_sync_gid ), "motorUtil: Sync group create failed" ); - - /* Wait on a (never signalled) event here, rather than suspending the thread, so as not to show up in the thread list as "SUSPENDED", which is usually a sign of a fault. */ @@ -327,16 +323,15 @@ static void moving(int callback_motor_index, chid callback_chid, if (motorUtil_debug) errlogPrintf("sending alldone = TRUE\n"); - ca_sg_array_put(moving_sync_gid, DBR_SHORT, 1, chid_alldone, &done); -// ca_put(DBR_SHORT, chid_alldone, &done); + ca_put(DBR_SHORT, chid_alldone, &done); old_alldone_value = new_alldone_value; } else { if (motorUtil_debug) errlogPrintf("sending alldone = FALSE\n"); - ca_sg_array_put(moving_sync_gid, DBR_SHORT, 1, chid_alldone, ¬_done); -// ca_put(DBR_SHORT, chid_alldone, ¬_done); + + ca_put(DBR_SHORT, chid_alldone, ¬_done); old_alldone_value = new_alldone_value; } } @@ -350,8 +345,7 @@ static void moving(int callback_motor_index, chid callback_chid, errlogPrintf("updating number of motors moving\n"); /* give $(P)moving the appropriate value */ - ca_sg_array_put(moving_sync_gid, DBR_LONG, 1, chid_moving, &numMotorsMoving); -// ca_put(DBR_LONG, chid_moving, &numMotorsMoving); + ca_put(DBR_LONG, chid_moving, &numMotorsMoving); /* Tell which motor's dmov changed */ sprintf(diffStr, "%c%s", diffChar, motorArray[callback_motor_index].name); @@ -364,7 +358,6 @@ static void moving(int callback_motor_index, chid callback_chid, /* send the ca_puts */ status = ca_flush_io(); - status = ca_sg_block(moving_sync_gid, 1.0); } From 0b605ea0044e6649cde49d2b06ecffcfb2bdda6f Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 23 Apr 2015 15:15:32 +0000 Subject: [PATCH 015/232] Use CA rather than PP on link Former-commit-id: ca15be336cf0b11bab2254f67d43f4a7aa8aa30a --- motorApp/Db/motorUtil.db | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index e64e7d232..4d2da6e23 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -50,13 +50,13 @@ record(dfanout, "$(P)_FAN") { field(OMSL, "supervisory") field(SELM, "All") -$(IFDMC01) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.A PP") -$(IFDMC02) field(OUTB, "$(PVPREFIX)CS:MOT:MOVING.B PP") -$(IFDMC03) field(OUTC, "$(PVPREFIX)CS:MOT:MOVING.C PP") -$(IFDMC04) field(OUTD, "$(PVPREFIX)CS:MOT:MOVING.D PP") -$(IFDMC05) field(OUTE, "$(PVPREFIX)CS:MOT:MOVING.E PP") -$(IFDMC06) field(OUTF, "$(PVPREFIX)CS:MOT:MOVING.F PP") -$(IFDMC07) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G PP") +$(IFDMC01) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.A CA") +$(IFDMC02) field(OUTB, "$(PVPREFIX)CS:MOT:MOVING.B CA") +$(IFDMC03) field(OUTC, "$(PVPREFIX)CS:MOT:MOVING.C CA") +$(IFDMC04) field(OUTD, "$(PVPREFIX)CS:MOT:MOVING.D CA") +$(IFDMC05) field(OUTE, "$(PVPREFIX)CS:MOT:MOVING.E CA") +$(IFDMC06) field(OUTF, "$(PVPREFIX)CS:MOT:MOVING.F CA") +$(IFDMC07) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G CA") } # force periodoc processing to catch up any missing, shouldn't @@ -66,6 +66,8 @@ record(fanout, "$(P)_FAN2") field(SCAN, "10 second") field(SELM, "All") field(LNK1, "$(P)_MOVING.PROC") + field(SDIS, "$(P)_MOVING.PACT") + field(DISV, 1) } # The number of motors moving in this IOC. From b7731ed67fe0cb6b3bc84bb298a543a70d092d4c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 3 Jul 2015 18:29:29 +0000 Subject: [PATCH 016/232] Add note about homing reset Former-commit-id: 0f680dc1a6b0f38b8e6b987d205e1e5b650d8a73 --- motorApp/NewportSrc/SMC100Driver.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/motorApp/NewportSrc/SMC100Driver.cpp b/motorApp/NewportSrc/SMC100Driver.cpp index b7793807e..be6c86782 100644 --- a/motorApp/NewportSrc/SMC100Driver.cpp +++ b/motorApp/NewportSrc/SMC100Driver.cpp @@ -197,6 +197,12 @@ asynStatus SMC100Axis::home(double minVelocity, double maxVelocity, double accel asynStatus status; // static const char *functionName = "SMC100Axis::home"; + // Must be in unreferenced state to home, so can only home once after a reset + // this code should force a reset and allow a rehome, but controller doesn't seem happy + // sprintf(pC_->outString_, "%1dRS", axisNo_ + 1); + // status = pC_->writeController(); + // epicsThreadSleep(5.0); + // set Home search velocity //sprintf(pC_->outString_, "%1dOH%f", axisNo_ + 1, maxVelocity); //status = pC_->writeController(); From b10a6b1a1e2c03c3c5f2d7aaa5b44088053758c2 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sun, 5 Jul 2015 14:13:41 +0000 Subject: [PATCH 017/232] Add default comment char for when macro is undefined Former-commit-id: c33654e1f83b720c07391f6c42f819ad88d6450b --- motorApp/Db/motorUtil.db | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 4d2da6e23..ef44b6f92 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -50,13 +50,14 @@ record(dfanout, "$(P)_FAN") { field(OMSL, "supervisory") field(SELM, "All") -$(IFDMC01) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.A CA") -$(IFDMC02) field(OUTB, "$(PVPREFIX)CS:MOT:MOVING.B CA") -$(IFDMC03) field(OUTC, "$(PVPREFIX)CS:MOT:MOVING.C CA") -$(IFDMC04) field(OUTD, "$(PVPREFIX)CS:MOT:MOVING.D CA") -$(IFDMC05) field(OUTE, "$(PVPREFIX)CS:MOT:MOVING.E CA") -$(IFDMC06) field(OUTF, "$(PVPREFIX)CS:MOT:MOVING.F CA") -$(IFDMC07) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G CA") +$(IFDMC01=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.A CA") +$(IFDMC02=#) field(OUTB, "$(PVPREFIX)CS:MOT:MOVING.B CA") +$(IFDMC03=#) field(OUTC, "$(PVPREFIX)CS:MOT:MOVING.C CA") +$(IFDMC04=#) field(OUTD, "$(PVPREFIX)CS:MOT:MOVING.D CA") +$(IFDMC05=#) field(OUTE, "$(PVPREFIX)CS:MOT:MOVING.E CA") +$(IFDMC06=#) field(OUTF, "$(PVPREFIX)CS:MOT:MOVING.F CA") +$(IFDMC07=#) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G CA") +$(IFDMC08=#) field(OUTH, "$(PVPREFIX)CS:MOT:MOVING.H CA") } # force periodoc processing to catch up any missing, shouldn't From 7bce8804b4dfedc95a37d11fd7876cc8a6b5fdf7 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 8 Jul 2015 12:18:10 +0000 Subject: [PATCH 018/232] Save SPMG for asyn motor Former-commit-id: 6486825996f4bfb4b7eadb3ae272d9a0ea46ae62 --- motorApp/Db/asyn_motor_positions.req | 2 +- motorApp/Db/basic_motor_settings.req | 2 +- motorApp/Db/motor_settings.req | 2 -- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/motorApp/Db/asyn_motor_positions.req b/motorApp/Db/asyn_motor_positions.req index 2d8666bb3..731a90302 100644 --- a/motorApp/Db/asyn_motor_positions.req +++ b/motorApp/Db/asyn_motor_positions.req @@ -1,2 +1,2 @@ # FILE... motor_positions.req -$(P)$(M).DVAL +#$(P)$(M).DVAL diff --git a/motorApp/Db/basic_motor_settings.req b/motorApp/Db/basic_motor_settings.req index d31669ad9..44280fcc5 100644 --- a/motorApp/Db/basic_motor_settings.req +++ b/motorApp/Db/basic_motor_settings.req @@ -49,4 +49,4 @@ $(P)$(M).FLNK $(P)$(M).RMOD $(P)$(M).ADEL $(P)$(M).MDEL - +$(P)$(M).SPMG diff --git a/motorApp/Db/motor_settings.req b/motorApp/Db/motor_settings.req index f8fb93776..57a0d9be6 100644 --- a/motorApp/Db/motor_settings.req +++ b/motorApp/Db/motor_settings.req @@ -5,6 +5,4 @@ # HeadURL: $URL$ file basic_motor_settings.req P=$(P),M=$(M) -$(P)$(M).RMOD $(P)$(M)_able.VAL -$(P)$(M).SPMG From 4615c144268f92d8938019dc76e6b2809dd38a83 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 8 Jul 2015 16:18:02 +0000 Subject: [PATCH 019/232] Implement moving motors for SMC100 and CONEXAGP Former-commit-id: 394229060786e10b6cef18cfc2fa42159e67a002 --- motorApp/Db/motorUtil.db | 2 ++ 1 file changed, 2 insertions(+) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index ef44b6f92..1db70878b 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -58,6 +58,8 @@ $(IFDMC05=#) field(OUTE, "$(PVPREFIX)CS:MOT:MOVING.E CA") $(IFDMC06=#) field(OUTF, "$(PVPREFIX)CS:MOT:MOVING.F CA") $(IFDMC07=#) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G CA") $(IFDMC08=#) field(OUTH, "$(PVPREFIX)CS:MOT:MOVING.H CA") +$(IFIOC_SMC100_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.A CA") +$(IFIOC_CONEXAGP_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.B CA") } # force periodoc processing to catch up any missing, shouldn't From 0f3b6e72309fe4e5a292abdc62d10a745ade3c19 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 18 Aug 2015 01:02:57 +0100 Subject: [PATCH 020/232] Fix RELEASE paths --- .gitattributes | 27 +++++++++++++++++++++++++ .gitignore | 11 ++++++++++ configure/RELEASE | 51 +++-------------------------------------------- 3 files changed, 41 insertions(+), 48 deletions(-) create mode 100644 .gitattributes create mode 100644 .gitignore diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 000000000..35b94aa8c --- /dev/null +++ b/.gitattributes @@ -0,0 +1,27 @@ +# Set the default behavior, in case people don't have core.autocrlf set. +* text=auto + +# Explicitly declare text files you want to always be normalized and converted +# to native line endings on checkout. +*.c text +*.cpp text +*.h text +*.hpp text +*.java text +*.sh text eol=lf +*.bat text +*.cmd text +*.db text +*.dbd text +*.template text +*.substitutions text + +# Declare files that will always have CRLF line endings on checkout. +*.sln text eol=crlf + +# Denote all files that are truly binary and should not be modified. +*.png binary +*.jpg binary +*.class binary +*.vi binary + diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..bbade29ad --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ +O.*/ +/db +/bin +/dbd +/include +/lib +/templates +envPaths +cdCommands +dllPath.bat +runIoc.bat diff --git a/configure/RELEASE b/configure/RELEASE index fb2c8479b..4915c302e 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -1,51 +1,6 @@ -# RELEASE - Location of external products -# -# Note: This file will be scanned to automatically generate -# include path definitions etc. -# The order of the following declarations will be retained -# within those definitions. -# -# Note that "#!" marks lines that can be uncommented. -# -# Add your declarations here - -SUPPORT= --include $(TOP)/../configure/SUPPORT.$(EPICS_HOST_ARCH) - -# If any motor controller communication mechanism besides the VME backplane is -# required, then ASYN must be defined. -# Recommended ASYN release: R4-21 -ASYN=$(SUPPORT)/asyn/4-26 - -# Need the sequencer and the bust record for the MM4005 and XPS trajectory scanning -# Recommended SNCSEQ release: R2-1-12 -SNCSEQ=$(SUPPORT)/seq/2-1-11 -# Recommended BUSY release: R1-5 -BUSY=$(SUPPORT)/busy/1-6-1 -CALC=$(SUPPORT)/calc/3-4 - -AUTOSAVE=$(SUPPORT)/autosave/R5_6_1 - -ONCRPC=$(SUPPORT)/oncrpc/2 -WAIT=$(SUPPORT)/calc/2-9 -SSCAN=$(SUPPORT)/sscan/2-8 - -# Recommended EPICS release: R3.14.12.1 -EPICS_BASE= --include $(TOP)/../configure/EPICS_BASE.$(EPICS_HOST_ARCH) - -# The following must be defined for the MXmotor device driver. -#!MX=$(SUPPORT)/mx/mx - -# The following support modules are required for the Hytec driver and for the examples in -# /motorExApp. To build examples, the top Makefile, -# /Makefile must also be edited. - -# Recommended IPAC release: R2-11 -IPAC=$(SUPPORT)/ipac/2-11 - -# The following is only needed for the motor examples in iocBoot. -MOTOR=$(SUPPORT)/motor/R6-9 +include $(TOP)/../../../configure/MASTER_RELEASE +# overrdie module definitions here +-include $(TOP)/configure/RELEASE.private include $(TOP)/../../../ISIS_CONFIG.$(EPICS_HOST_ARCH) From 2600ab00b2a218b55b153782c5cbc4512e00916d Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 18 Aug 2015 11:20:38 +0100 Subject: [PATCH 021/232] Update .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index bbade29ad..91cc72927 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,4 @@ envPaths cdCommands dllPath.bat runIoc.bat +iocBoot/iocmotorSim/motor.substitutions.local From c4a3432a3d4f7bc354690e83ea45f37c84d938f2 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 19 Aug 2015 23:31:09 +0100 Subject: [PATCH 022/232] Fix line endings --- .../iocmotorSim/motor.substitutions.allstop | 28 +- motorApp/AerotechSrc/ParameterId.h | 1536 +- motorApp/AerotechSrc/doCommand.ab | 470 +- motorApp/HytecSrc/README_Hy8601Asyn | 164 +- motorApp/MotorSrc/asynMotorAxis.h | 156 +- motorApp/MotorSrc/asynMotorController.h | 602 +- motorApp/NewportSrc/XPS_C8_errors.h | 214 +- motorApp/NewportSrc/hxp_drivers.cpp | 13808 ++++++++-------- motorApp/NewportSrc/hxp_drivers.h | 362 +- motorApp/NewportSrc/hxp_errors.h | 196 +- 10 files changed, 8768 insertions(+), 8768 deletions(-) diff --git a/iocBoot/iocmotorSim/motor.substitutions.allstop b/iocBoot/iocmotorSim/motor.substitutions.allstop index a7f28a15a..1ebd42ad8 100644 --- a/iocBoot/iocmotorSim/motor.substitutions.allstop +++ b/iocBoot/iocmotorSim/motor.substitutions.allstop @@ -1,14 +1,14 @@ -file "../../db/stop_all.db" -{ -pattern -{P, N, M, AS} -{"NDW1298:sjb99183:", 1, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 2, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 3, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 4, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} - -{"NDW1298:sjb99183:", 5, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 6, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 7, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 8, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -} +file "../../db/stop_all.db" +{ +pattern +{P, N, M, AS} +{"NDW1298:sjb99183:", 1, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 2, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 3, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 4, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} + +{"NDW1298:sjb99183:", 5, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 6, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 7, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +{"NDW1298:sjb99183:", 8, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} +} diff --git a/motorApp/AerotechSrc/ParameterId.h b/motorApp/AerotechSrc/ParameterId.h index a2da635c1..e527f8a52 100644 --- a/motorApp/AerotechSrc/ParameterId.h +++ b/motorApp/AerotechSrc/ParameterId.h @@ -1,768 +1,768 @@ -/// \file ParameterId.h -/// \brief File contains parameter identifiers -/// -/// This file is version dependant and needs to match the rest of the software - -#ifndef __PARAMETER_ID_H__ -#define __PARAMETER_ID_H__ - - -/// \brief Represents a parameter identifier -typedef enum { - /// \brief The AxisType parameter - PARAMETERID_AxisType = ( (0 << 24) | 0 ), - /// \brief The ReverseMotionDirection parameter - PARAMETERID_ReverseMotionDirection = ( (0 << 24) | 1 ), - /// \brief The CountsPerUnit parameter - PARAMETERID_CountsPerUnit = ( (0 << 24) | 2 ), - /// \brief The ServoRate parameter - PARAMETERID_ServoRate = ( (0 << 24) | 3 ), - /// \brief The ServoSetup parameter - PARAMETERID_ServoSetup = ( (0 << 24) | 4 ), - /// \brief The GainKpos parameter - PARAMETERID_GainKpos = ( (0 << 24) | 5 ), - /// \brief The GainKi parameter - PARAMETERID_GainKi = ( (0 << 24) | 6 ), - /// \brief The GainKp parameter - PARAMETERID_GainKp = ( (0 << 24) | 7 ), - /// \brief The GainVff parameter - PARAMETERID_GainVff = ( (0 << 24) | 8 ), - /// \brief The GainAff parameter - PARAMETERID_GainAff = ( (0 << 24) | 9 ), - /// \brief The GainKv parameter - PARAMETERID_GainKv = ( (0 << 24) | 10 ), - /// \brief The GainKpi parameter - PARAMETERID_GainKpi = ( (0 << 24) | 11 ), - /// \brief The ServoFilter0CoeffN0 parameter - PARAMETERID_ServoFilter0CoeffN0 = ( (0 << 24) | 12 ), - /// \brief The ServoFilter0CoeffN1 parameter - PARAMETERID_ServoFilter0CoeffN1 = ( (0 << 24) | 13 ), - /// \brief The ServoFilter0CoeffN2 parameter - PARAMETERID_ServoFilter0CoeffN2 = ( (0 << 24) | 14 ), - /// \brief The ServoFilter0CoeffD1 parameter - PARAMETERID_ServoFilter0CoeffD1 = ( (0 << 24) | 15 ), - /// \brief The ServoFilter0CoeffD2 parameter - PARAMETERID_ServoFilter0CoeffD2 = ( (0 << 24) | 16 ), - /// \brief The ServoFilter1CoeffN0 parameter - PARAMETERID_ServoFilter1CoeffN0 = ( (0 << 24) | 17 ), - /// \brief The ServoFilter1CoeffN1 parameter - PARAMETERID_ServoFilter1CoeffN1 = ( (0 << 24) | 18 ), - /// \brief The ServoFilter1CoeffN2 parameter - PARAMETERID_ServoFilter1CoeffN2 = ( (0 << 24) | 19 ), - /// \brief The ServoFilter1CoeffD1 parameter - PARAMETERID_ServoFilter1CoeffD1 = ( (0 << 24) | 20 ), - /// \brief The ServoFilter1CoeffD2 parameter - PARAMETERID_ServoFilter1CoeffD2 = ( (0 << 24) | 21 ), - /// \brief The AmplifierDeadtime parameter - PARAMETERID_AmplifierDeadtime = ( (0 << 24) | 22 ), - /// \brief The RolloverCounts parameter - PARAMETERID_RolloverCounts = ( (0 << 24) | 23 ), - /// \brief The CurrentGainKi parameter - PARAMETERID_CurrentGainKi = ( (0 << 24) | 24 ), - /// \brief The CurrentGainKp parameter - PARAMETERID_CurrentGainKp = ( (0 << 24) | 25 ), - /// \brief The FaultMask parameter - PARAMETERID_FaultMask = ( (0 << 24) | 26 ), - /// \brief The FaultMaskDisable parameter - PARAMETERID_FaultMaskDisable = ( (0 << 24) | 27 ), - /// \brief The FaultMaskDecel parameter - PARAMETERID_FaultMaskDecel = ( (0 << 24) | 28 ), - /// \brief The EnableBrakeControl parameter - PARAMETERID_EnableBrakeControl = ( (0 << 24) | 29 ), - /// \brief The FaultMaskOutput parameter - PARAMETERID_FaultMaskOutput = ( (0 << 24) | 30 ), - /// \brief The ESTOPFaultInput parameter - PARAMETERID_ESTOPFaultInput = ( (0 << 24) | 31 ), - /// \brief The PositionErrorThreshold parameter - PARAMETERID_PositionErrorThreshold = ( (0 << 24) | 32 ), - /// \brief The AverageCurrentThreshold parameter - PARAMETERID_AverageCurrentThreshold = ( (0 << 24) | 33 ), - /// \brief The AverageCurrentTime parameter - PARAMETERID_AverageCurrentTime = ( (0 << 24) | 34 ), - /// \brief The VelocityCommandThreshold parameter - PARAMETERID_VelocityCommandThreshold = ( (0 << 24) | 35 ), - /// \brief The VelocityErrorThreshold parameter - PARAMETERID_VelocityErrorThreshold = ( (0 << 24) | 36 ), - /// \brief The SoftwareLimitLow parameter - PARAMETERID_SoftwareLimitLow = ( (0 << 24) | 37 ), - /// \brief The SoftwareLimitHigh parameter - PARAMETERID_SoftwareLimitHigh = ( (0 << 24) | 38 ), - /// \brief The MaxCurrentClamp parameter - PARAMETERID_MaxCurrentClamp = ( (0 << 24) | 39 ), - /// \brief The InPositionDistance parameter - PARAMETERID_InPositionDistance = ( (0 << 24) | 40 ), - /// \brief The MotorType parameter - PARAMETERID_MotorType = ( (0 << 24) | 41 ), - /// \brief The CyclesPerRev parameter - PARAMETERID_CyclesPerRev = ( (0 << 24) | 42 ), - /// \brief The CountsPerRev parameter - PARAMETERID_CountsPerRev = ( (0 << 24) | 43 ), - /// \brief The CommutationOffset parameter - PARAMETERID_CommutationOffset = ( (0 << 24) | 44 ), - /// \brief The AutoMsetTime parameter - PARAMETERID_AutoMsetTime = ( (0 << 24) | 45 ), - /// \brief The AutoMsetCurrent parameter - PARAMETERID_AutoMsetCurrent = ( (0 << 24) | 46 ), - /// \brief The PositionFeedbackType parameter - PARAMETERID_PositionFeedbackType = ( (0 << 24) | 47 ), - /// \brief The PositionFeedbackChannel parameter - PARAMETERID_PositionFeedbackChannel = ( (0 << 24) | 48 ), - /// \brief The VelocityFeedbackType parameter - PARAMETERID_VelocityFeedbackType = ( (0 << 24) | 49 ), - /// \brief The VelocityFeedbackChannel parameter - PARAMETERID_VelocityFeedbackChannel = ( (0 << 24) | 50 ), - /// \brief The EncoderMultiplicationFactor parameter - PARAMETERID_EncoderMultiplicationFactor = ( (0 << 24) | 51 ), - /// \brief The EncoderSineGain parameter - PARAMETERID_EncoderSineGain = ( (0 << 24) | 52 ), - /// \brief The EncoderSineOffset parameter - PARAMETERID_EncoderSineOffset = ( (0 << 24) | 53 ), - /// \brief The EncoderCosineGain parameter - PARAMETERID_EncoderCosineGain = ( (0 << 24) | 54 ), - /// \brief The EncoderCosineOffset parameter - PARAMETERID_EncoderCosineOffset = ( (0 << 24) | 55 ), - /// \brief The EncoderPhase parameter - PARAMETERID_EncoderPhase = ( (0 << 24) | 56 ), - /// \brief The GantryMasterAxis parameter - PARAMETERID_GantryMasterAxis = ( (0 << 24) | 57 ), - /// \brief The LimitDecelDistance parameter - PARAMETERID_LimitDecelDistance = ( (0 << 24) | 59 ), - /// \brief The LimitDebounceTime parameter - PARAMETERID_LimitDebounceTime = ( (0 << 24) | 60 ), - /// \brief The EndOfTravelLimitSetup parameter - PARAMETERID_EndOfTravelLimitSetup = ( (0 << 24) | 61 ), - /// \brief The BacklashDistance parameter - PARAMETERID_BacklashDistance = ( (0 << 24) | 62 ), - /// \brief The FaultOutputSetup parameter - PARAMETERID_FaultOutputSetup = ( (0 << 24) | 63 ), - /// \brief The FaultOutputState parameter - PARAMETERID_FaultOutputState = ( (0 << 24) | 64 ), - /// \brief The IOSetup parameter - PARAMETERID_IOSetup = ( (0 << 24) | 65 ), - /// \brief The BrakeOutput parameter - PARAMETERID_BrakeOutput = ( (0 << 24) | 66 ), - /// \brief The EncoderDivider parameter - PARAMETERID_EncoderDivider = ( (0 << 24) | 67 ), - /// \brief The ExternalFaultDigitalInput parameter - PARAMETERID_ExternalFaultDigitalInput = ( (0 << 24) | 68 ), - /// \brief The BrakeDisableDelay parameter - PARAMETERID_BrakeDisableDelay = ( (0 << 24) | 69 ), - /// \brief The MaxJogDistance parameter - PARAMETERID_MaxJogDistance = ( (0 << 24) | 70 ), - /// \brief The DefaultSpeed parameter - PARAMETERID_DefaultSpeed = ( (0 << 24) | 71 ), - /// \brief The DefaultRampRate parameter - PARAMETERID_DefaultRampRate = ( (0 << 24) | 72 ), - /// \brief The AbortDecelRate parameter - PARAMETERID_AbortDecelRate = ( (0 << 24) | 73 ), - /// \brief The HomeType parameter - PARAMETERID_HomeType = ( (0 << 24) | 74 ), - /// \brief The HomeSetup parameter - PARAMETERID_HomeSetup = ( (0 << 24) | 75 ), - /// \brief The HomeSpeed parameter - PARAMETERID_HomeSpeed = ( (0 << 24) | 76 ), - /// \brief The HomeOffset parameter - PARAMETERID_HomeOffset = ( (0 << 24) | 77 ), - /// \brief The HomeRampRate parameter - PARAMETERID_HomeRampRate = ( (0 << 24) | 78 ), - /// \brief The DefaultWaitMode parameter - PARAMETERID_DefaultWaitMode = ( (0 << 24) | 79 ), - /// \brief The DefaultSCurve parameter - PARAMETERID_DefaultSCurve = ( (0 << 24) | 80 ), - /// \brief The DataCollectionPoints parameter - PARAMETERID_DataCollectionPoints = ( (0 << 24) | 81 ), - /// \brief The StepperResolution parameter - PARAMETERID_StepperResolution = ( (0 << 24) | 83 ), - /// \brief The StepperRunningCurrent parameter - PARAMETERID_StepperRunningCurrent = ( (0 << 24) | 84 ), - /// \brief The StepperHoldingCurrent parameter - PARAMETERID_StepperHoldingCurrent = ( (0 << 24) | 85 ), - /// \brief The StepperVerificationSpeed parameter - PARAMETERID_StepperVerificationSpeed = ( (0 << 24) | 86 ), - /// \brief The LimitDebounceDistance parameter - PARAMETERID_LimitDebounceDistance = ( (0 << 24) | 87 ), - /// \brief The ServoFilter2CoeffN0 parameter - PARAMETERID_ServoFilter2CoeffN0 = ( (0 << 24) | 88 ), - /// \brief The ServoFilter2CoeffN1 parameter - PARAMETERID_ServoFilter2CoeffN1 = ( (0 << 24) | 89 ), - /// \brief The ServoFilter2CoeffN2 parameter - PARAMETERID_ServoFilter2CoeffN2 = ( (0 << 24) | 90 ), - /// \brief The ServoFilter2CoeffD1 parameter - PARAMETERID_ServoFilter2CoeffD1 = ( (0 << 24) | 91 ), - /// \brief The ServoFilter2CoeffD2 parameter - PARAMETERID_ServoFilter2CoeffD2 = ( (0 << 24) | 92 ), - /// \brief The ServoFilter3CoeffN0 parameter - PARAMETERID_ServoFilter3CoeffN0 = ( (0 << 24) | 93 ), - /// \brief The ServoFilter3CoeffN1 parameter - PARAMETERID_ServoFilter3CoeffN1 = ( (0 << 24) | 94 ), - /// \brief The ServoFilter3CoeffN2 parameter - PARAMETERID_ServoFilter3CoeffN2 = ( (0 << 24) | 95 ), - /// \brief The ServoFilter3CoeffD1 parameter - PARAMETERID_ServoFilter3CoeffD1 = ( (0 << 24) | 96 ), - /// \brief The ServoFilter3CoeffD2 parameter - PARAMETERID_ServoFilter3CoeffD2 = ( (0 << 24) | 97 ), - /// \brief The GearCamSource parameter - PARAMETERID_GearCamSource = ( (0 << 24) | 98 ), - /// \brief The GearCamIndex parameter - PARAMETERID_GearCamIndex = ( (0 << 24) | 99 ), - /// \brief The GearCamScaleFactor parameter - PARAMETERID_GearCamScaleFactor = ( (0 << 24) | 100 ), - /// \brief The GearCamAnalogDeadband parameter - PARAMETERID_GearCamAnalogDeadband = ( (0 << 24) | 105 ), - /// \brief The PrintBufferSize parameter - PARAMETERID_PrintBufferSize = ( (0 << 24) | 106 ), - /// \brief The SerialPort0XonCharacter parameter - PARAMETERID_SerialPort0XonCharacter = ( (0 << 24) | 109 ), - /// \brief The SerialPort0XoffCharacter parameter - PARAMETERID_SerialPort0XoffCharacter = ( (0 << 24) | 110 ), - /// \brief The SerialPort0BaudRate parameter - PARAMETERID_SerialPort0BaudRate = ( (0 << 24) | 111 ), - /// \brief The SerialPort0Setup parameter - PARAMETERID_SerialPort0Setup = ( (0 << 24) | 112 ), - /// \brief The TaskExecutionSetup parameter - PARAMETERID_TaskExecutionSetup = ( (0 << 24) | 113 ), - /// \brief The CodeSize parameter - PARAMETERID_CodeSize = ( (0 << 24) | 114 ), - /// \brief The DataSize parameter - PARAMETERID_DataSize = ( (0 << 24) | 115 ), - /// \brief The StackSize parameter - PARAMETERID_StackSize = ( (0 << 24) | 116 ), - /// \brief The AutoRunProgram parameter - PARAMETERID_AutoRunProgram = ( (0 << 24) | 118 ), - /// \brief The MaxJogSpeed parameter - PARAMETERID_MaxJogSpeed = ( (0 << 24) | 123 ), - /// \brief The GlobalIntegers parameter - PARAMETERID_GlobalIntegers = ( (0 << 24) | 124 ), - /// \brief The GlobalDoubles parameter - PARAMETERID_GlobalDoubles = ( (0 << 24) | 125 ), - /// \brief The DecimalPlaces parameter - PARAMETERID_DecimalPlaces = ( (0 << 24) | 126 ), - /// \brief The TaskErrorAbortAxes parameter - PARAMETERID_TaskErrorAbortAxes = ( (0 << 24) | 127 ), - /// \brief The CalibrationFile1D parameter - PARAMETERID_CalibrationFile1D = ( (0 << 24) | 128 ), - /// \brief The UnitsName parameter - PARAMETERID_UnitsName = ( (0 << 24) | 129 ), - /// \brief The Socket2RemoteIPAddress parameter - PARAMETERID_Socket2RemoteIPAddress = ( (0 << 24) | 130 ), - /// \brief The Socket2Port parameter - PARAMETERID_Socket2Port = ( (0 << 24) | 131 ), - /// \brief The Socket2Setup parameter - PARAMETERID_Socket2Setup = ( (0 << 24) | 132 ), - /// \brief The Socket2TransmissionSize parameter - PARAMETERID_Socket2TransmissionSize = ( (0 << 24) | 133 ), - /// \brief The Socket3RemoteIPAddress parameter - PARAMETERID_Socket3RemoteIPAddress = ( (0 << 24) | 134 ), - /// \brief The Socket3Port parameter - PARAMETERID_Socket3Port = ( (0 << 24) | 135 ), - /// \brief The Socket3Setup parameter - PARAMETERID_Socket3Setup = ( (0 << 24) | 136 ), - /// \brief The Socket3TransmissionSize parameter - PARAMETERID_Socket3TransmissionSize = ( (0 << 24) | 137 ), - /// \brief The Socket2Timeout parameter - PARAMETERID_Socket2Timeout = ( (0 << 24) | 138 ), - /// \brief The Socket3Timeout parameter - PARAMETERID_Socket3Timeout = ( (0 << 24) | 139 ), - /// \brief The AxisName parameter - PARAMETERID_AxisName = ( (0 << 24) | 140 ), - /// \brief The UserInteger0 parameter - PARAMETERID_UserInteger0 = ( (0 << 24) | 141 ), - /// \brief The UserInteger1 parameter - PARAMETERID_UserInteger1 = ( (0 << 24) | 142 ), - /// \brief The UserDouble0 parameter - PARAMETERID_UserDouble0 = ( (0 << 24) | 143 ), - /// \brief The UserDouble1 parameter - PARAMETERID_UserDouble1 = ( (0 << 24) | 144 ), - /// \brief The UserString0 parameter - PARAMETERID_UserString0 = ( (0 << 24) | 145 ), - /// \brief The UserString1 parameter - PARAMETERID_UserString1 = ( (0 << 24) | 146 ), - /// \brief The EnDatEncoderSetup parameter - PARAMETERID_EnDatEncoderSetup = ( (0 << 24) | 147 ), - /// \brief The EnDatEncoderResolution parameter - PARAMETERID_EnDatEncoderResolution = ( (0 << 24) | 148 ), - /// \brief The EnDatEncoderTurns parameter - PARAMETERID_EnDatEncoderTurns = ( (0 << 24) | 149 ), - /// \brief The CommandSetup parameter - PARAMETERID_CommandSetup = ( (0 << 24) | 150 ), - /// \brief The SerialPort1XonCharacter parameter - PARAMETERID_SerialPort1XonCharacter = ( (0 << 24) | 152 ), - /// \brief The SerialPort1XoffCharacter parameter - PARAMETERID_SerialPort1XoffCharacter = ( (0 << 24) | 153 ), - /// \brief The SerialPort1BaudRate parameter - PARAMETERID_SerialPort1BaudRate = ( (0 << 24) | 154 ), - /// \brief The SerialPort1Setup parameter - PARAMETERID_SerialPort1Setup = ( (0 << 24) | 155 ), - /// \brief The RequiredAxes parameter - PARAMETERID_RequiredAxes = ( (0 << 24) | 156 ), - /// \brief The JoystickInput1MinVoltage parameter - PARAMETERID_JoystickInput1MinVoltage = ( (0 << 24) | 157 ), - /// \brief The JoystickInput1MaxVoltage parameter - PARAMETERID_JoystickInput1MaxVoltage = ( (0 << 24) | 158 ), - /// \brief The JoystickInput1Deadband parameter - PARAMETERID_JoystickInput1Deadband = ( (0 << 24) | 159 ), - /// \brief The JoystickInput0MinVoltage parameter - PARAMETERID_JoystickInput0MinVoltage = ( (0 << 24) | 160 ), - /// \brief The JoystickInput0MaxVoltage parameter - PARAMETERID_JoystickInput0MaxVoltage = ( (0 << 24) | 161 ), - /// \brief The JoystickInput0Deadband parameter - PARAMETERID_JoystickInput0Deadband = ( (0 << 24) | 162 ), - /// \brief The JoystickLowSpeed parameter - PARAMETERID_JoystickLowSpeed = ( (0 << 24) | 163 ), - /// \brief The JoystickHighSpeed parameter - PARAMETERID_JoystickHighSpeed = ( (0 << 24) | 164 ), - /// \brief The JoystickSetup parameter - PARAMETERID_JoystickSetup = ( (0 << 24) | 165 ), - /// \brief The HomePositionSet parameter - PARAMETERID_HomePositionSet = ( (0 << 24) | 166 ), - /// \brief The TaskTerminationAxes parameter - PARAMETERID_TaskTerminationAxes = ( (0 << 24) | 167 ), - /// \brief The TaskStopAbortAxes parameter - PARAMETERID_TaskStopAbortAxes = ( (0 << 24) | 168 ), - /// \brief The CalibrationFile2D parameter - PARAMETERID_CalibrationFile2D = ( (0 << 24) | 169 ), - /// \brief The FaultMaskDisableDelay parameter - PARAMETERID_FaultMaskDisableDelay = ( (0 << 24) | 170 ), - /// \brief The DefaultCoordinatedSpeed parameter - PARAMETERID_DefaultCoordinatedSpeed = ( (0 << 24) | 171 ), - /// \brief The DefaultCoordinatedRampRate parameter - PARAMETERID_DefaultCoordinatedRampRate = ( (0 << 24) | 172 ), - /// \brief The DefaultDependentCoordinatedRampRate parameter - PARAMETERID_DefaultDependentCoordinatedRampRate = ( (0 << 24) | 173 ), - /// \brief The GpibTerminatingCharacter parameter - PARAMETERID_GpibTerminatingCharacter = ( (0 << 24) | 174 ), - /// \brief The GpibPrimaryAddress parameter - PARAMETERID_GpibPrimaryAddress = ( (0 << 24) | 175 ), - /// \brief The GpibParallelResponse parameter - PARAMETERID_GpibParallelResponse = ( (0 << 24) | 176 ), - /// \brief The CommandTerminatingCharacter parameter - PARAMETERID_CommandTerminatingCharacter = ( (0 << 24) | 177 ), - /// \brief The CommandSuccessCharacter parameter - PARAMETERID_CommandSuccessCharacter = ( (0 << 24) | 178 ), - /// \brief The CommandInvalidCharacter parameter - PARAMETERID_CommandInvalidCharacter = ( (0 << 24) | 179 ), - /// \brief The CommandFaultCharacter parameter - PARAMETERID_CommandFaultCharacter = ( (0 << 24) | 180 ), - /// \brief The FaultAbortAxes parameter - PARAMETERID_FaultAbortAxes = ( (0 << 24) | 182 ), - /// \brief The HarmonicCancellation0Type parameter - PARAMETERID_HarmonicCancellation0Type = ( (0 << 24) | 185 ), - /// \brief The HarmonicCancellation0Period parameter - PARAMETERID_HarmonicCancellation0Period = ( (0 << 24) | 186 ), - /// \brief The HarmonicCancellation0Gain parameter - PARAMETERID_HarmonicCancellation0Gain = ( (0 << 24) | 188 ), - /// \brief The HarmonicCancellation0Phase parameter - PARAMETERID_HarmonicCancellation0Phase = ( (0 << 24) | 189 ), - /// \brief The HarmonicCancellation1Type parameter - PARAMETERID_HarmonicCancellation1Type = ( (0 << 24) | 190 ), - /// \brief The HarmonicCancellation1Period parameter - PARAMETERID_HarmonicCancellation1Period = ( (0 << 24) | 191 ), - /// \brief The HarmonicCancellation1Gain parameter - PARAMETERID_HarmonicCancellation1Gain = ( (0 << 24) | 193 ), - /// \brief The HarmonicCancellation1Phase parameter - PARAMETERID_HarmonicCancellation1Phase = ( (0 << 24) | 194 ), - /// \brief The HarmonicCancellation2Type parameter - PARAMETERID_HarmonicCancellation2Type = ( (0 << 24) | 195 ), - /// \brief The HarmonicCancellation2Period parameter - PARAMETERID_HarmonicCancellation2Period = ( (0 << 24) | 196 ), - /// \brief The HarmonicCancellation2Gain parameter - PARAMETERID_HarmonicCancellation2Gain = ( (0 << 24) | 198 ), - /// \brief The HarmonicCancellation2Phase parameter - PARAMETERID_HarmonicCancellation2Phase = ( (0 << 24) | 199 ), - /// \brief The CommandTimeout parameter - PARAMETERID_CommandTimeout = ( (0 << 24) | 202 ), - /// \brief The CommandTimeoutCharacter parameter - PARAMETERID_CommandTimeoutCharacter = ( (0 << 24) | 203 ), - /// \brief The ResolverReferenceGain parameter - PARAMETERID_ResolverReferenceGain = ( (0 << 24) | 204 ), - /// \brief The ResolverSetup parameter - PARAMETERID_ResolverSetup = ( (0 << 24) | 205 ), - /// \brief The ResolverReferencePhase parameter - PARAMETERID_ResolverReferencePhase = ( (0 << 24) | 206 ), - /// \brief The SoftwareLimitSetup parameter - PARAMETERID_SoftwareLimitSetup = ( (0 << 24) | 210 ), - /// \brief The SSINet1Setup parameter - PARAMETERID_SSINet1Setup = ( (0 << 24) | 211 ), - /// \brief The SSINet2Setup parameter - PARAMETERID_SSINet2Setup = ( (0 << 24) | 212 ), - /// \brief The EmulatedQuadratureDivider parameter - PARAMETERID_EmulatedQuadratureDivider = ( (0 << 24) | 213 ), - /// \brief The HarmonicCancellation3Type parameter - PARAMETERID_HarmonicCancellation3Type = ( (0 << 24) | 214 ), - /// \brief The HarmonicCancellation3Period parameter - PARAMETERID_HarmonicCancellation3Period = ( (0 << 24) | 215 ), - /// \brief The HarmonicCancellation3Gain parameter - PARAMETERID_HarmonicCancellation3Gain = ( (0 << 24) | 217 ), - /// \brief The HarmonicCancellation3Phase parameter - PARAMETERID_HarmonicCancellation3Phase = ( (0 << 24) | 218 ), - /// \brief The HarmonicCancellation4Type parameter - PARAMETERID_HarmonicCancellation4Type = ( (0 << 24) | 219 ), - /// \brief The HarmonicCancellation4Period parameter - PARAMETERID_HarmonicCancellation4Period = ( (0 << 24) | 220 ), - /// \brief The HarmonicCancellation4Gain parameter - PARAMETERID_HarmonicCancellation4Gain = ( (0 << 24) | 222 ), - /// \brief The HarmonicCancellation4Phase parameter - PARAMETERID_HarmonicCancellation4Phase = ( (0 << 24) | 223 ), - /// \brief The EnhancedThroughputChannel parameter - PARAMETERID_EnhancedThroughputChannel = ( (0 << 24) | 224 ), - /// \brief The EnhancedThroughputGain parameter - PARAMETERID_EnhancedThroughputGain = ( (0 << 24) | 225 ), - /// \brief The HarmonicCancellationSetup parameter - PARAMETERID_HarmonicCancellationSetup = ( (0 << 24) | 226 ), - /// \brief The EnhancedThroughputCurrentClamp parameter - PARAMETERID_EnhancedThroughputCurrentClamp = ( (0 << 24) | 227 ), - /// \brief The Analog0Filter0CoeffN0 parameter - PARAMETERID_Analog0Filter0CoeffN0 = ( (0 << 24) | 228 ), - /// \brief The Analog0Filter0CoeffN1 parameter - PARAMETERID_Analog0Filter0CoeffN1 = ( (0 << 24) | 229 ), - /// \brief The Analog0Filter0CoeffN2 parameter - PARAMETERID_Analog0Filter0CoeffN2 = ( (0 << 24) | 230 ), - /// \brief The Analog0Filter0CoeffD1 parameter - PARAMETERID_Analog0Filter0CoeffD1 = ( (0 << 24) | 231 ), - /// \brief The Analog0Filter0CoeffD2 parameter - PARAMETERID_Analog0Filter0CoeffD2 = ( (0 << 24) | 232 ), - /// \brief The Analog0Filter1CoeffN0 parameter - PARAMETERID_Analog0Filter1CoeffN0 = ( (0 << 24) | 233 ), - /// \brief The Analog0Filter1CoeffN1 parameter - PARAMETERID_Analog0Filter1CoeffN1 = ( (0 << 24) | 234 ), - /// \brief The Analog0Filter1CoeffN2 parameter - PARAMETERID_Analog0Filter1CoeffN2 = ( (0 << 24) | 235 ), - /// \brief The Analog0Filter1CoeffD1 parameter - PARAMETERID_Analog0Filter1CoeffD1 = ( (0 << 24) | 236 ), - /// \brief The Analog0Filter1CoeffD2 parameter - PARAMETERID_Analog0Filter1CoeffD2 = ( (0 << 24) | 237 ), - /// \brief The Analog1Filter0CoeffN0 parameter - PARAMETERID_Analog1Filter0CoeffN0 = ( (0 << 24) | 238 ), - /// \brief The Analog1Filter0CoeffN1 parameter - PARAMETERID_Analog1Filter0CoeffN1 = ( (0 << 24) | 239 ), - /// \brief The Analog1Filter0CoeffN2 parameter - PARAMETERID_Analog1Filter0CoeffN2 = ( (0 << 24) | 240 ), - /// \brief The Analog1Filter0CoeffD1 parameter - PARAMETERID_Analog1Filter0CoeffD1 = ( (0 << 24) | 241 ), - /// \brief The Analog1Filter0CoeffD2 parameter - PARAMETERID_Analog1Filter0CoeffD2 = ( (0 << 24) | 242 ), - /// \brief The Analog1Filter1CoeffN0 parameter - PARAMETERID_Analog1Filter1CoeffN0 = ( (0 << 24) | 243 ), - /// \brief The Analog1Filter1CoeffN1 parameter - PARAMETERID_Analog1Filter1CoeffN1 = ( (0 << 24) | 244 ), - /// \brief The Analog1Filter1CoeffN2 parameter - PARAMETERID_Analog1Filter1CoeffN2 = ( (0 << 24) | 245 ), - /// \brief The Analog1Filter1CoeffD1 parameter - PARAMETERID_Analog1Filter1CoeffD1 = ( (0 << 24) | 246 ), - /// \brief The Analog1Filter1CoeffD2 parameter - PARAMETERID_Analog1Filter1CoeffD2 = ( (0 << 24) | 247 ), - /// \brief The GlobalStrings parameter - PARAMETERID_GlobalStrings = ( (0 << 24) | 248 ), - /// \brief The DefaultCoordinatedRampMode parameter - PARAMETERID_DefaultCoordinatedRampMode = ( (0 << 24) | 249 ), - /// \brief The DefaultCoordinatedRampTime parameter - PARAMETERID_DefaultCoordinatedRampTime = ( (0 << 24) | 250 ), - /// \brief The DefaultCoordinatedRampDistance parameter - PARAMETERID_DefaultCoordinatedRampDistance = ( (0 << 24) | 251 ), - /// \brief The DefaultRampMode parameter - PARAMETERID_DefaultRampMode = ( (0 << 24) | 252 ), - /// \brief The DefaultRampTime parameter - PARAMETERID_DefaultRampTime = ( (0 << 24) | 253 ), - /// \brief The DefaultRampDistance parameter - PARAMETERID_DefaultRampDistance = ( (0 << 24) | 254 ), - /// \brief The ServoFilterSetup parameter - PARAMETERID_ServoFilterSetup = ( (0 << 24) | 255 ), - /// \brief The FeedbackSetup parameter - PARAMETERID_FeedbackSetup = ( (0 << 24) | 256 ), - /// \brief The EncoderMultiplierSetup parameter - PARAMETERID_EncoderMultiplierSetup = ( (0 << 24) | 257 ), - /// \brief The FaultSetup parameter - PARAMETERID_FaultSetup = ( (0 << 24) | 258 ), - /// \brief The ThresholdScheduleSetup parameter - PARAMETERID_ThresholdScheduleSetup = ( (0 << 24) | 259 ), - /// \brief The ThresholdRegion2High parameter - PARAMETERID_ThresholdRegion2High = ( (0 << 24) | 260 ), - /// \brief The ThresholdRegion2Low parameter - PARAMETERID_ThresholdRegion2Low = ( (0 << 24) | 261 ), - /// \brief The ThresholdRegion3GainKpos parameter - PARAMETERID_ThresholdRegion3GainKpos = ( (0 << 24) | 262 ), - /// \brief The ThresholdRegion3GainKp parameter - PARAMETERID_ThresholdRegion3GainKp = ( (0 << 24) | 263 ), - /// \brief The ThresholdRegion3GainKi parameter - PARAMETERID_ThresholdRegion3GainKi = ( (0 << 24) | 264 ), - /// \brief The ThresholdRegion3GainKpi parameter - PARAMETERID_ThresholdRegion3GainKpi = ( (0 << 24) | 265 ), - /// \brief The ThresholdRegion4High parameter - PARAMETERID_ThresholdRegion4High = ( (0 << 24) | 266 ), - /// \brief The ThresholdRegion4Low parameter - PARAMETERID_ThresholdRegion4Low = ( (0 << 24) | 267 ), - /// \brief The ThresholdRegion5GainKpos parameter - PARAMETERID_ThresholdRegion5GainKpos = ( (0 << 24) | 268 ), - /// \brief The ThresholdRegion5GainKp parameter - PARAMETERID_ThresholdRegion5GainKp = ( (0 << 24) | 269 ), - /// \brief The ThresholdRegion5GainKi parameter - PARAMETERID_ThresholdRegion5GainKi = ( (0 << 24) | 270 ), - /// \brief The ThresholdRegion5GainKpi parameter - PARAMETERID_ThresholdRegion5GainKpi = ( (0 << 24) | 271 ), - /// \brief The DynamicScheduleSetup parameter - PARAMETERID_DynamicScheduleSetup = ( (0 << 24) | 272 ), - /// \brief The DynamicGainKposScale parameter - PARAMETERID_DynamicGainKposScale = ( (0 << 24) | 273 ), - /// \brief The DynamicGainKpScale parameter - PARAMETERID_DynamicGainKpScale = ( (0 << 24) | 274 ), - /// \brief The DynamicGainKiScale parameter - PARAMETERID_DynamicGainKiScale = ( (0 << 24) | 275 ), - /// \brief The ServoFilter4CoeffN0 parameter - PARAMETERID_ServoFilter4CoeffN0 = ( (0 << 24) | 276 ), - /// \brief The ServoFilter4CoeffN1 parameter - PARAMETERID_ServoFilter4CoeffN1 = ( (0 << 24) | 277 ), - /// \brief The ServoFilter4CoeffN2 parameter - PARAMETERID_ServoFilter4CoeffN2 = ( (0 << 24) | 278 ), - /// \brief The ServoFilter4CoeffD1 parameter - PARAMETERID_ServoFilter4CoeffD1 = ( (0 << 24) | 279 ), - /// \brief The ServoFilter4CoeffD2 parameter - PARAMETERID_ServoFilter4CoeffD2 = ( (0 << 24) | 280 ), - /// \brief The ServoFilter5CoeffN0 parameter - PARAMETERID_ServoFilter5CoeffN0 = ( (0 << 24) | 281 ), - /// \brief The ServoFilter5CoeffN1 parameter - PARAMETERID_ServoFilter5CoeffN1 = ( (0 << 24) | 282 ), - /// \brief The ServoFilter5CoeffN2 parameter - PARAMETERID_ServoFilter5CoeffN2 = ( (0 << 24) | 283 ), - /// \brief The ServoFilter5CoeffD1 parameter - PARAMETERID_ServoFilter5CoeffD1 = ( (0 << 24) | 284 ), - /// \brief The ServoFilter5CoeffD2 parameter - PARAMETERID_ServoFilter5CoeffD2 = ( (0 << 24) | 285 ), - /// \brief The ServoFilter6CoeffN0 parameter - PARAMETERID_ServoFilter6CoeffN0 = ( (0 << 24) | 286 ), - /// \brief The ServoFilter6CoeffN1 parameter - PARAMETERID_ServoFilter6CoeffN1 = ( (0 << 24) | 287 ), - /// \brief The ServoFilter6CoeffN2 parameter - PARAMETERID_ServoFilter6CoeffN2 = ( (0 << 24) | 288 ), - /// \brief The ServoFilter6CoeffD1 parameter - PARAMETERID_ServoFilter6CoeffD1 = ( (0 << 24) | 289 ), - /// \brief The ServoFilter6CoeffD2 parameter - PARAMETERID_ServoFilter6CoeffD2 = ( (0 << 24) | 290 ), - /// \brief The ServoFilter7CoeffN0 parameter - PARAMETERID_ServoFilter7CoeffN0 = ( (0 << 24) | 291 ), - /// \brief The ServoFilter7CoeffN1 parameter - PARAMETERID_ServoFilter7CoeffN1 = ( (0 << 24) | 292 ), - /// \brief The ServoFilter7CoeffN2 parameter - PARAMETERID_ServoFilter7CoeffN2 = ( (0 << 24) | 293 ), - /// \brief The ServoFilter7CoeffD1 parameter - PARAMETERID_ServoFilter7CoeffD1 = ( (0 << 24) | 294 ), - /// \brief The ServoFilter7CoeffD2 parameter - PARAMETERID_ServoFilter7CoeffD2 = ( (0 << 24) | 295 ), - /// \brief The LinearAmpMaxPower parameter - PARAMETERID_LinearAmpMaxPower = ( (0 << 24) | 296 ), - /// \brief The LinearAmpDeratingFactor parameter - PARAMETERID_LinearAmpDeratingFactor = ( (0 << 24) | 297 ), - /// \brief The LinearAmpBusVoltage parameter - PARAMETERID_LinearAmpBusVoltage = ( (0 << 24) | 298 ), - /// \brief The MotorResistance parameter - PARAMETERID_MotorResistance = ( (0 << 24) | 299 ), - /// \brief The MotorBackEMFConstant parameter - PARAMETERID_MotorBackEMFConstant = ( (0 << 24) | 300 ), - /// \brief The GantrySetup parameter - PARAMETERID_GantrySetup = ( (0 << 24) | 302 ), - /// \brief The RolloverMode parameter - PARAMETERID_RolloverMode = ( (0 << 24) | 303 ), - /// \brief The ResolverCoarseChannel parameter - PARAMETERID_ResolverCoarseChannel = ( (0 << 24) | 306 ), - /// \brief The ResolverFeedbackRatio parameter - PARAMETERID_ResolverFeedbackRatio = ( (0 << 24) | 307 ), - /// \brief The ResolverFeedbackOffset parameter - PARAMETERID_ResolverFeedbackOffset = ( (0 << 24) | 308 ), - /// \brief The InPositionTime parameter - PARAMETERID_InPositionTime = ( (0 << 24) | 319 ), - /// \brief The ExternalFaultAnalogInput parameter - PARAMETERID_ExternalFaultAnalogInput = ( (0 << 24) | 424 ), - /// \brief The ExternalFaultThreshold parameter - PARAMETERID_ExternalFaultThreshold = ( (0 << 24) | 425 ), - /// \brief The DisplayAxes parameter - PARAMETERID_DisplayAxes = ( (0 << 24) | 426 ), - /// \brief The DefaultDependentCoordinatedSpeed parameter - PARAMETERID_DefaultDependentCoordinatedSpeed = ( (0 << 24) | 427 ), - /// \brief The AnalogFilterSetup parameter - PARAMETERID_AnalogFilterSetup = ( (0 << 24) | 482 ), - /// \brief The ModbusMasterSlaveIPAddress parameter - PARAMETERID_ModbusMasterSlaveIPAddress = ( (0 << 24) | 489 ), - /// \brief The ModbusMasterSlavePort parameter - PARAMETERID_ModbusMasterSlavePort = ( (0 << 24) | 490 ), - /// \brief The ModbusMasterSlaveID parameter - PARAMETERID_ModbusMasterSlaveID = ( (0 << 24) | 491 ), - /// \brief The ModbusMasterInputWords parameter - PARAMETERID_ModbusMasterInputWords = ( (0 << 24) | 492 ), - /// \brief The ModbusMasterOutputWords parameter - PARAMETERID_ModbusMasterOutputWords = ( (0 << 24) | 493 ), - /// \brief The ModbusMasterInputBits parameter - PARAMETERID_ModbusMasterInputBits = ( (0 << 24) | 494 ), - /// \brief The ModbusMasterOutputBits parameter - PARAMETERID_ModbusMasterOutputBits = ( (0 << 24) | 495 ), - /// \brief The ModbusMasterSetup parameter - PARAMETERID_ModbusMasterSetup = ( (0 << 24) | 496 ), - /// \brief The ModbusMasterVirtualInputs parameter - PARAMETERID_ModbusMasterVirtualInputs = ( (0 << 24) | 499 ), - /// \brief The ModbusMasterVirtualOutputs parameter - PARAMETERID_ModbusMasterVirtualOutputs = ( (0 << 24) | 500 ), - /// \brief The ModbusMasterOutputWordsSections parameter - PARAMETERID_ModbusMasterOutputWordsSections = ( (0 << 24) | 501 ), - /// \brief The ModbusMasterOutputBitsSections parameter - PARAMETERID_ModbusMasterOutputBitsSections = ( (0 << 24) | 502 ), - /// \brief The ModbusMasterRWReadOffset parameter - PARAMETERID_ModbusMasterRWReadOffset = ( (0 << 24) | 503 ), - /// \brief The ModbusMasterInputWordsOffset parameter - PARAMETERID_ModbusMasterInputWordsOffset = ( (0 << 24) | 504 ), - /// \brief The ModbusMasterOutputWordsOffset parameter - PARAMETERID_ModbusMasterOutputWordsOffset = ( (0 << 24) | 505 ), - /// \brief The ModbusMasterInputBitsOffset parameter - PARAMETERID_ModbusMasterInputBitsOffset = ( (0 << 24) | 506 ), - /// \brief The ModbusMasterOutputBitsOffset parameter - PARAMETERID_ModbusMasterOutputBitsOffset = ( (0 << 24) | 507 ), - /// \brief The ModbusMasterStatusWordsOffset parameter - PARAMETERID_ModbusMasterStatusWordsOffset = ( (0 << 24) | 508 ), - /// \brief The ModbusMasterStatusBitsOffset parameter - PARAMETERID_ModbusMasterStatusBitsOffset = ( (0 << 24) | 509 ), - /// \brief The ModbusMasterVirtualInputsOffset parameter - PARAMETERID_ModbusMasterVirtualInputsOffset = ( (0 << 24) | 510 ), - /// \brief The ModbusMasterVirtualOutputsOffset parameter - PARAMETERID_ModbusMasterVirtualOutputsOffset = ( (0 << 24) | 511 ), - /// \brief The ModbusMasterRWWriteOffset parameter - PARAMETERID_ModbusMasterRWWriteOffset = ( (0 << 24) | 512 ), - /// \brief The ModbusMasterFunctions parameter - PARAMETERID_ModbusMasterFunctions = ( (0 << 24) | 513 ), - /// \brief The ModbusMasterSlaveType parameter - PARAMETERID_ModbusMasterSlaveType = ( (0 << 24) | 514 ), - /// \brief The ModbusSlaveUnitID parameter - PARAMETERID_ModbusSlaveUnitID = ( (0 << 24) | 516 ), - /// \brief The ModbusSlaveInputWords parameter - PARAMETERID_ModbusSlaveInputWords = ( (0 << 24) | 517 ), - /// \brief The ModbusSlaveOutputWords parameter - PARAMETERID_ModbusSlaveOutputWords = ( (0 << 24) | 518 ), - /// \brief The ModbusSlaveInputBits parameter - PARAMETERID_ModbusSlaveInputBits = ( (0 << 24) | 519 ), - /// \brief The ModbusSlaveOutputBits parameter - PARAMETERID_ModbusSlaveOutputBits = ( (0 << 24) | 520 ), - /// \brief The ModbusSlaveInputWordsOffset parameter - PARAMETERID_ModbusSlaveInputWordsOffset = ( (0 << 24) | 521 ), - /// \brief The ModbusSlaveOutputWordsOffset parameter - PARAMETERID_ModbusSlaveOutputWordsOffset = ( (0 << 24) | 522 ), - /// \brief The ModbusSlaveInputBitsOffset parameter - PARAMETERID_ModbusSlaveInputBitsOffset = ( (0 << 24) | 523 ), - /// \brief The ModbusSlaveOutputBitsOffset parameter - PARAMETERID_ModbusSlaveOutputBitsOffset = ( (0 << 24) | 524 ), - /// \brief The ModbusSlaveRWReadOffset parameter - PARAMETERID_ModbusSlaveRWReadOffset = ( (0 << 24) | 525 ), - /// \brief The ModbusSlaveRWWriteOffset parameter - PARAMETERID_ModbusSlaveRWWriteOffset = ( (0 << 24) | 526 ), - /// \brief The CurrentOffsetA parameter - PARAMETERID_CurrentOffsetA = ( (0 << 24) | 662 ), - /// \brief The CurrentOffsetB parameter - PARAMETERID_CurrentOffsetB = ( (0 << 24) | 663 ), - /// \brief The CommandShaperSetup parameter - PARAMETERID_CommandShaperSetup = ( (0 << 24) | 666 ), - /// \brief The CommandShaperTime00 parameter - PARAMETERID_CommandShaperTime00 = ( (0 << 24) | 667 ), - /// \brief The CommandShaperTime01 parameter - PARAMETERID_CommandShaperTime01 = ( (0 << 24) | 668 ), - /// \brief The CommandShaperTime02 parameter - PARAMETERID_CommandShaperTime02 = ( (0 << 24) | 669 ), - /// \brief The CommandShaperTime03 parameter - PARAMETERID_CommandShaperTime03 = ( (0 << 24) | 670 ), - /// \brief The CommandShaperTime04 parameter - PARAMETERID_CommandShaperTime04 = ( (0 << 24) | 671 ), - /// \brief The CommandShaperTime05 parameter - PARAMETERID_CommandShaperTime05 = ( (0 << 24) | 672 ), - /// \brief The CommandShaperTime06 parameter - PARAMETERID_CommandShaperTime06 = ( (0 << 24) | 673 ), - /// \brief The CommandShaperTime07 parameter - PARAMETERID_CommandShaperTime07 = ( (0 << 24) | 674 ), - /// \brief The CommandShaperTime08 parameter - PARAMETERID_CommandShaperTime08 = ( (0 << 24) | 675 ), - /// \brief The CommandShaperTime09 parameter - PARAMETERID_CommandShaperTime09 = ( (0 << 24) | 676 ), - /// \brief The CommandShaperTime10 parameter - PARAMETERID_CommandShaperTime10 = ( (0 << 24) | 677 ), - /// \brief The CommandShaperTime11 parameter - PARAMETERID_CommandShaperTime11 = ( (0 << 24) | 678 ), - /// \brief The CommandShaperTime12 parameter - PARAMETERID_CommandShaperTime12 = ( (0 << 24) | 679 ), - /// \brief The CommandShaperTime13 parameter - PARAMETERID_CommandShaperTime13 = ( (0 << 24) | 680 ), - /// \brief The CommandShaperTime14 parameter - PARAMETERID_CommandShaperTime14 = ( (0 << 24) | 681 ), - /// \brief The CommandShaperTime15 parameter - PARAMETERID_CommandShaperTime15 = ( (0 << 24) | 682 ), - /// \brief The CommandShaperCoeff00 parameter - PARAMETERID_CommandShaperCoeff00 = ( (0 << 24) | 683 ), - /// \brief The CommandShaperCoeff01 parameter - PARAMETERID_CommandShaperCoeff01 = ( (0 << 24) | 684 ), - /// \brief The CommandShaperCoeff02 parameter - PARAMETERID_CommandShaperCoeff02 = ( (0 << 24) | 685 ), - /// \brief The CommandShaperCoeff03 parameter - PARAMETERID_CommandShaperCoeff03 = ( (0 << 24) | 686 ), - /// \brief The CommandShaperCoeff04 parameter - PARAMETERID_CommandShaperCoeff04 = ( (0 << 24) | 687 ), - /// \brief The CommandShaperCoeff05 parameter - PARAMETERID_CommandShaperCoeff05 = ( (0 << 24) | 688 ), - /// \brief The CommandShaperCoeff06 parameter - PARAMETERID_CommandShaperCoeff06 = ( (0 << 24) | 689 ), - /// \brief The CommandShaperCoeff07 parameter - PARAMETERID_CommandShaperCoeff07 = ( (0 << 24) | 690 ), - /// \brief The CommandShaperCoeff08 parameter - PARAMETERID_CommandShaperCoeff08 = ( (0 << 24) | 691 ), - /// \brief The CommandShaperCoeff09 parameter - PARAMETERID_CommandShaperCoeff09 = ( (0 << 24) | 692 ), - /// \brief The CommandShaperCoeff10 parameter - PARAMETERID_CommandShaperCoeff10 = ( (0 << 24) | 693 ), - /// \brief The CommandShaperCoeff11 parameter - PARAMETERID_CommandShaperCoeff11 = ( (0 << 24) | 694 ), - /// \brief The CommandShaperCoeff12 parameter - PARAMETERID_CommandShaperCoeff12 = ( (0 << 24) | 695 ), - /// \brief The CommandShaperCoeff13 parameter - PARAMETERID_CommandShaperCoeff13 = ( (0 << 24) | 696 ), - /// \brief The CommandShaperCoeff14 parameter - PARAMETERID_CommandShaperCoeff14 = ( (0 << 24) | 697 ), - /// \brief The CommandShaperCoeff15 parameter - PARAMETERID_CommandShaperCoeff15 = ( (0 << 24) | 698 ), - /// \brief The CommandShaper0Type parameter - PARAMETERID_CommandShaper0Type = ( (0 << 24) | 703 ), - /// \brief The CommandShaper0Frequency parameter - PARAMETERID_CommandShaper0Frequency = ( (0 << 24) | 704 ), - /// \brief The CommandShaper0Damping parameter - PARAMETERID_CommandShaper0Damping = ( (0 << 24) | 705 ), - /// \brief The CommandShaper1Type parameter - PARAMETERID_CommandShaper1Type = ( (0 << 24) | 706 ), - /// \brief The CommandShaper1Frequency parameter - PARAMETERID_CommandShaper1Frequency = ( (0 << 24) | 707 ), - /// \brief The CommandShaper1Damping parameter - PARAMETERID_CommandShaper1Damping = ( (0 << 24) | 708 ), - /// \brief The ResoluteEncoderSetup parameter - PARAMETERID_ResoluteEncoderSetup = ( (0 << 24) | 715 ), - /// \brief The ResoluteEncoderResolution parameter - PARAMETERID_ResoluteEncoderResolution = ( (0 << 24) | 716 ), - /// \brief The ResoluteEncoderUserResolution parameter - PARAMETERID_ResoluteEncoderUserResolution = ( (0 << 24) | 717 ), - /// \brief The AutofocusInput parameter - PARAMETERID_AutofocusInput = ( (0 << 24) | 721 ), - /// \brief The AutofocusTarget parameter - PARAMETERID_AutofocusTarget = ( (0 << 24) | 722 ), - /// \brief The AutofocusDeadband parameter - PARAMETERID_AutofocusDeadband = ( (0 << 24) | 723 ), - /// \brief The AutofocusGainKi parameter - PARAMETERID_AutofocusGainKi = ( (0 << 24) | 724 ), - /// \brief The AutofocusGainKp parameter - PARAMETERID_AutofocusGainKp = ( (0 << 24) | 725 ), - /// \brief The AutofocusLimitLow parameter - PARAMETERID_AutofocusLimitLow = ( (0 << 24) | 726 ), - /// \brief The AutofocusLimitHigh parameter - PARAMETERID_AutofocusLimitHigh = ( (0 << 24) | 727 ), - /// \brief The AutofocusSpeedClamp parameter - PARAMETERID_AutofocusSpeedClamp = ( (0 << 24) | 728 ), - /// \brief The AutofocusHoldInput parameter - PARAMETERID_AutofocusHoldInput = ( (0 << 24) | 729 ), - /// \brief The AutofocusSetup parameter - PARAMETERID_AutofocusSetup = ( (0 << 24) | 730 ), - /// \brief The ExternalSyncFrequency parameter - PARAMETERID_ExternalSyncFrequency = ( (0 << 24) | 731 ), - /// \brief The GainPff parameter - PARAMETERID_GainPff = ( (0 << 24) | 762 ), - /// \brief The AutofocusInitialRampTime parameter - PARAMETERID_AutofocusInitialRampTime = ( (0 << 24) | 763 ), -} PARAMETERID; - -#endif // __PARAMETER_ID_H__ +/// \file ParameterId.h +/// \brief File contains parameter identifiers +/// +/// This file is version dependant and needs to match the rest of the software + +#ifndef __PARAMETER_ID_H__ +#define __PARAMETER_ID_H__ + + +/// \brief Represents a parameter identifier +typedef enum { + /// \brief The AxisType parameter + PARAMETERID_AxisType = ( (0 << 24) | 0 ), + /// \brief The ReverseMotionDirection parameter + PARAMETERID_ReverseMotionDirection = ( (0 << 24) | 1 ), + /// \brief The CountsPerUnit parameter + PARAMETERID_CountsPerUnit = ( (0 << 24) | 2 ), + /// \brief The ServoRate parameter + PARAMETERID_ServoRate = ( (0 << 24) | 3 ), + /// \brief The ServoSetup parameter + PARAMETERID_ServoSetup = ( (0 << 24) | 4 ), + /// \brief The GainKpos parameter + PARAMETERID_GainKpos = ( (0 << 24) | 5 ), + /// \brief The GainKi parameter + PARAMETERID_GainKi = ( (0 << 24) | 6 ), + /// \brief The GainKp parameter + PARAMETERID_GainKp = ( (0 << 24) | 7 ), + /// \brief The GainVff parameter + PARAMETERID_GainVff = ( (0 << 24) | 8 ), + /// \brief The GainAff parameter + PARAMETERID_GainAff = ( (0 << 24) | 9 ), + /// \brief The GainKv parameter + PARAMETERID_GainKv = ( (0 << 24) | 10 ), + /// \brief The GainKpi parameter + PARAMETERID_GainKpi = ( (0 << 24) | 11 ), + /// \brief The ServoFilter0CoeffN0 parameter + PARAMETERID_ServoFilter0CoeffN0 = ( (0 << 24) | 12 ), + /// \brief The ServoFilter0CoeffN1 parameter + PARAMETERID_ServoFilter0CoeffN1 = ( (0 << 24) | 13 ), + /// \brief The ServoFilter0CoeffN2 parameter + PARAMETERID_ServoFilter0CoeffN2 = ( (0 << 24) | 14 ), + /// \brief The ServoFilter0CoeffD1 parameter + PARAMETERID_ServoFilter0CoeffD1 = ( (0 << 24) | 15 ), + /// \brief The ServoFilter0CoeffD2 parameter + PARAMETERID_ServoFilter0CoeffD2 = ( (0 << 24) | 16 ), + /// \brief The ServoFilter1CoeffN0 parameter + PARAMETERID_ServoFilter1CoeffN0 = ( (0 << 24) | 17 ), + /// \brief The ServoFilter1CoeffN1 parameter + PARAMETERID_ServoFilter1CoeffN1 = ( (0 << 24) | 18 ), + /// \brief The ServoFilter1CoeffN2 parameter + PARAMETERID_ServoFilter1CoeffN2 = ( (0 << 24) | 19 ), + /// \brief The ServoFilter1CoeffD1 parameter + PARAMETERID_ServoFilter1CoeffD1 = ( (0 << 24) | 20 ), + /// \brief The ServoFilter1CoeffD2 parameter + PARAMETERID_ServoFilter1CoeffD2 = ( (0 << 24) | 21 ), + /// \brief The AmplifierDeadtime parameter + PARAMETERID_AmplifierDeadtime = ( (0 << 24) | 22 ), + /// \brief The RolloverCounts parameter + PARAMETERID_RolloverCounts = ( (0 << 24) | 23 ), + /// \brief The CurrentGainKi parameter + PARAMETERID_CurrentGainKi = ( (0 << 24) | 24 ), + /// \brief The CurrentGainKp parameter + PARAMETERID_CurrentGainKp = ( (0 << 24) | 25 ), + /// \brief The FaultMask parameter + PARAMETERID_FaultMask = ( (0 << 24) | 26 ), + /// \brief The FaultMaskDisable parameter + PARAMETERID_FaultMaskDisable = ( (0 << 24) | 27 ), + /// \brief The FaultMaskDecel parameter + PARAMETERID_FaultMaskDecel = ( (0 << 24) | 28 ), + /// \brief The EnableBrakeControl parameter + PARAMETERID_EnableBrakeControl = ( (0 << 24) | 29 ), + /// \brief The FaultMaskOutput parameter + PARAMETERID_FaultMaskOutput = ( (0 << 24) | 30 ), + /// \brief The ESTOPFaultInput parameter + PARAMETERID_ESTOPFaultInput = ( (0 << 24) | 31 ), + /// \brief The PositionErrorThreshold parameter + PARAMETERID_PositionErrorThreshold = ( (0 << 24) | 32 ), + /// \brief The AverageCurrentThreshold parameter + PARAMETERID_AverageCurrentThreshold = ( (0 << 24) | 33 ), + /// \brief The AverageCurrentTime parameter + PARAMETERID_AverageCurrentTime = ( (0 << 24) | 34 ), + /// \brief The VelocityCommandThreshold parameter + PARAMETERID_VelocityCommandThreshold = ( (0 << 24) | 35 ), + /// \brief The VelocityErrorThreshold parameter + PARAMETERID_VelocityErrorThreshold = ( (0 << 24) | 36 ), + /// \brief The SoftwareLimitLow parameter + PARAMETERID_SoftwareLimitLow = ( (0 << 24) | 37 ), + /// \brief The SoftwareLimitHigh parameter + PARAMETERID_SoftwareLimitHigh = ( (0 << 24) | 38 ), + /// \brief The MaxCurrentClamp parameter + PARAMETERID_MaxCurrentClamp = ( (0 << 24) | 39 ), + /// \brief The InPositionDistance parameter + PARAMETERID_InPositionDistance = ( (0 << 24) | 40 ), + /// \brief The MotorType parameter + PARAMETERID_MotorType = ( (0 << 24) | 41 ), + /// \brief The CyclesPerRev parameter + PARAMETERID_CyclesPerRev = ( (0 << 24) | 42 ), + /// \brief The CountsPerRev parameter + PARAMETERID_CountsPerRev = ( (0 << 24) | 43 ), + /// \brief The CommutationOffset parameter + PARAMETERID_CommutationOffset = ( (0 << 24) | 44 ), + /// \brief The AutoMsetTime parameter + PARAMETERID_AutoMsetTime = ( (0 << 24) | 45 ), + /// \brief The AutoMsetCurrent parameter + PARAMETERID_AutoMsetCurrent = ( (0 << 24) | 46 ), + /// \brief The PositionFeedbackType parameter + PARAMETERID_PositionFeedbackType = ( (0 << 24) | 47 ), + /// \brief The PositionFeedbackChannel parameter + PARAMETERID_PositionFeedbackChannel = ( (0 << 24) | 48 ), + /// \brief The VelocityFeedbackType parameter + PARAMETERID_VelocityFeedbackType = ( (0 << 24) | 49 ), + /// \brief The VelocityFeedbackChannel parameter + PARAMETERID_VelocityFeedbackChannel = ( (0 << 24) | 50 ), + /// \brief The EncoderMultiplicationFactor parameter + PARAMETERID_EncoderMultiplicationFactor = ( (0 << 24) | 51 ), + /// \brief The EncoderSineGain parameter + PARAMETERID_EncoderSineGain = ( (0 << 24) | 52 ), + /// \brief The EncoderSineOffset parameter + PARAMETERID_EncoderSineOffset = ( (0 << 24) | 53 ), + /// \brief The EncoderCosineGain parameter + PARAMETERID_EncoderCosineGain = ( (0 << 24) | 54 ), + /// \brief The EncoderCosineOffset parameter + PARAMETERID_EncoderCosineOffset = ( (0 << 24) | 55 ), + /// \brief The EncoderPhase parameter + PARAMETERID_EncoderPhase = ( (0 << 24) | 56 ), + /// \brief The GantryMasterAxis parameter + PARAMETERID_GantryMasterAxis = ( (0 << 24) | 57 ), + /// \brief The LimitDecelDistance parameter + PARAMETERID_LimitDecelDistance = ( (0 << 24) | 59 ), + /// \brief The LimitDebounceTime parameter + PARAMETERID_LimitDebounceTime = ( (0 << 24) | 60 ), + /// \brief The EndOfTravelLimitSetup parameter + PARAMETERID_EndOfTravelLimitSetup = ( (0 << 24) | 61 ), + /// \brief The BacklashDistance parameter + PARAMETERID_BacklashDistance = ( (0 << 24) | 62 ), + /// \brief The FaultOutputSetup parameter + PARAMETERID_FaultOutputSetup = ( (0 << 24) | 63 ), + /// \brief The FaultOutputState parameter + PARAMETERID_FaultOutputState = ( (0 << 24) | 64 ), + /// \brief The IOSetup parameter + PARAMETERID_IOSetup = ( (0 << 24) | 65 ), + /// \brief The BrakeOutput parameter + PARAMETERID_BrakeOutput = ( (0 << 24) | 66 ), + /// \brief The EncoderDivider parameter + PARAMETERID_EncoderDivider = ( (0 << 24) | 67 ), + /// \brief The ExternalFaultDigitalInput parameter + PARAMETERID_ExternalFaultDigitalInput = ( (0 << 24) | 68 ), + /// \brief The BrakeDisableDelay parameter + PARAMETERID_BrakeDisableDelay = ( (0 << 24) | 69 ), + /// \brief The MaxJogDistance parameter + PARAMETERID_MaxJogDistance = ( (0 << 24) | 70 ), + /// \brief The DefaultSpeed parameter + PARAMETERID_DefaultSpeed = ( (0 << 24) | 71 ), + /// \brief The DefaultRampRate parameter + PARAMETERID_DefaultRampRate = ( (0 << 24) | 72 ), + /// \brief The AbortDecelRate parameter + PARAMETERID_AbortDecelRate = ( (0 << 24) | 73 ), + /// \brief The HomeType parameter + PARAMETERID_HomeType = ( (0 << 24) | 74 ), + /// \brief The HomeSetup parameter + PARAMETERID_HomeSetup = ( (0 << 24) | 75 ), + /// \brief The HomeSpeed parameter + PARAMETERID_HomeSpeed = ( (0 << 24) | 76 ), + /// \brief The HomeOffset parameter + PARAMETERID_HomeOffset = ( (0 << 24) | 77 ), + /// \brief The HomeRampRate parameter + PARAMETERID_HomeRampRate = ( (0 << 24) | 78 ), + /// \brief The DefaultWaitMode parameter + PARAMETERID_DefaultWaitMode = ( (0 << 24) | 79 ), + /// \brief The DefaultSCurve parameter + PARAMETERID_DefaultSCurve = ( (0 << 24) | 80 ), + /// \brief The DataCollectionPoints parameter + PARAMETERID_DataCollectionPoints = ( (0 << 24) | 81 ), + /// \brief The StepperResolution parameter + PARAMETERID_StepperResolution = ( (0 << 24) | 83 ), + /// \brief The StepperRunningCurrent parameter + PARAMETERID_StepperRunningCurrent = ( (0 << 24) | 84 ), + /// \brief The StepperHoldingCurrent parameter + PARAMETERID_StepperHoldingCurrent = ( (0 << 24) | 85 ), + /// \brief The StepperVerificationSpeed parameter + PARAMETERID_StepperVerificationSpeed = ( (0 << 24) | 86 ), + /// \brief The LimitDebounceDistance parameter + PARAMETERID_LimitDebounceDistance = ( (0 << 24) | 87 ), + /// \brief The ServoFilter2CoeffN0 parameter + PARAMETERID_ServoFilter2CoeffN0 = ( (0 << 24) | 88 ), + /// \brief The ServoFilter2CoeffN1 parameter + PARAMETERID_ServoFilter2CoeffN1 = ( (0 << 24) | 89 ), + /// \brief The ServoFilter2CoeffN2 parameter + PARAMETERID_ServoFilter2CoeffN2 = ( (0 << 24) | 90 ), + /// \brief The ServoFilter2CoeffD1 parameter + PARAMETERID_ServoFilter2CoeffD1 = ( (0 << 24) | 91 ), + /// \brief The ServoFilter2CoeffD2 parameter + PARAMETERID_ServoFilter2CoeffD2 = ( (0 << 24) | 92 ), + /// \brief The ServoFilter3CoeffN0 parameter + PARAMETERID_ServoFilter3CoeffN0 = ( (0 << 24) | 93 ), + /// \brief The ServoFilter3CoeffN1 parameter + PARAMETERID_ServoFilter3CoeffN1 = ( (0 << 24) | 94 ), + /// \brief The ServoFilter3CoeffN2 parameter + PARAMETERID_ServoFilter3CoeffN2 = ( (0 << 24) | 95 ), + /// \brief The ServoFilter3CoeffD1 parameter + PARAMETERID_ServoFilter3CoeffD1 = ( (0 << 24) | 96 ), + /// \brief The ServoFilter3CoeffD2 parameter + PARAMETERID_ServoFilter3CoeffD2 = ( (0 << 24) | 97 ), + /// \brief The GearCamSource parameter + PARAMETERID_GearCamSource = ( (0 << 24) | 98 ), + /// \brief The GearCamIndex parameter + PARAMETERID_GearCamIndex = ( (0 << 24) | 99 ), + /// \brief The GearCamScaleFactor parameter + PARAMETERID_GearCamScaleFactor = ( (0 << 24) | 100 ), + /// \brief The GearCamAnalogDeadband parameter + PARAMETERID_GearCamAnalogDeadband = ( (0 << 24) | 105 ), + /// \brief The PrintBufferSize parameter + PARAMETERID_PrintBufferSize = ( (0 << 24) | 106 ), + /// \brief The SerialPort0XonCharacter parameter + PARAMETERID_SerialPort0XonCharacter = ( (0 << 24) | 109 ), + /// \brief The SerialPort0XoffCharacter parameter + PARAMETERID_SerialPort0XoffCharacter = ( (0 << 24) | 110 ), + /// \brief The SerialPort0BaudRate parameter + PARAMETERID_SerialPort0BaudRate = ( (0 << 24) | 111 ), + /// \brief The SerialPort0Setup parameter + PARAMETERID_SerialPort0Setup = ( (0 << 24) | 112 ), + /// \brief The TaskExecutionSetup parameter + PARAMETERID_TaskExecutionSetup = ( (0 << 24) | 113 ), + /// \brief The CodeSize parameter + PARAMETERID_CodeSize = ( (0 << 24) | 114 ), + /// \brief The DataSize parameter + PARAMETERID_DataSize = ( (0 << 24) | 115 ), + /// \brief The StackSize parameter + PARAMETERID_StackSize = ( (0 << 24) | 116 ), + /// \brief The AutoRunProgram parameter + PARAMETERID_AutoRunProgram = ( (0 << 24) | 118 ), + /// \brief The MaxJogSpeed parameter + PARAMETERID_MaxJogSpeed = ( (0 << 24) | 123 ), + /// \brief The GlobalIntegers parameter + PARAMETERID_GlobalIntegers = ( (0 << 24) | 124 ), + /// \brief The GlobalDoubles parameter + PARAMETERID_GlobalDoubles = ( (0 << 24) | 125 ), + /// \brief The DecimalPlaces parameter + PARAMETERID_DecimalPlaces = ( (0 << 24) | 126 ), + /// \brief The TaskErrorAbortAxes parameter + PARAMETERID_TaskErrorAbortAxes = ( (0 << 24) | 127 ), + /// \brief The CalibrationFile1D parameter + PARAMETERID_CalibrationFile1D = ( (0 << 24) | 128 ), + /// \brief The UnitsName parameter + PARAMETERID_UnitsName = ( (0 << 24) | 129 ), + /// \brief The Socket2RemoteIPAddress parameter + PARAMETERID_Socket2RemoteIPAddress = ( (0 << 24) | 130 ), + /// \brief The Socket2Port parameter + PARAMETERID_Socket2Port = ( (0 << 24) | 131 ), + /// \brief The Socket2Setup parameter + PARAMETERID_Socket2Setup = ( (0 << 24) | 132 ), + /// \brief The Socket2TransmissionSize parameter + PARAMETERID_Socket2TransmissionSize = ( (0 << 24) | 133 ), + /// \brief The Socket3RemoteIPAddress parameter + PARAMETERID_Socket3RemoteIPAddress = ( (0 << 24) | 134 ), + /// \brief The Socket3Port parameter + PARAMETERID_Socket3Port = ( (0 << 24) | 135 ), + /// \brief The Socket3Setup parameter + PARAMETERID_Socket3Setup = ( (0 << 24) | 136 ), + /// \brief The Socket3TransmissionSize parameter + PARAMETERID_Socket3TransmissionSize = ( (0 << 24) | 137 ), + /// \brief The Socket2Timeout parameter + PARAMETERID_Socket2Timeout = ( (0 << 24) | 138 ), + /// \brief The Socket3Timeout parameter + PARAMETERID_Socket3Timeout = ( (0 << 24) | 139 ), + /// \brief The AxisName parameter + PARAMETERID_AxisName = ( (0 << 24) | 140 ), + /// \brief The UserInteger0 parameter + PARAMETERID_UserInteger0 = ( (0 << 24) | 141 ), + /// \brief The UserInteger1 parameter + PARAMETERID_UserInteger1 = ( (0 << 24) | 142 ), + /// \brief The UserDouble0 parameter + PARAMETERID_UserDouble0 = ( (0 << 24) | 143 ), + /// \brief The UserDouble1 parameter + PARAMETERID_UserDouble1 = ( (0 << 24) | 144 ), + /// \brief The UserString0 parameter + PARAMETERID_UserString0 = ( (0 << 24) | 145 ), + /// \brief The UserString1 parameter + PARAMETERID_UserString1 = ( (0 << 24) | 146 ), + /// \brief The EnDatEncoderSetup parameter + PARAMETERID_EnDatEncoderSetup = ( (0 << 24) | 147 ), + /// \brief The EnDatEncoderResolution parameter + PARAMETERID_EnDatEncoderResolution = ( (0 << 24) | 148 ), + /// \brief The EnDatEncoderTurns parameter + PARAMETERID_EnDatEncoderTurns = ( (0 << 24) | 149 ), + /// \brief The CommandSetup parameter + PARAMETERID_CommandSetup = ( (0 << 24) | 150 ), + /// \brief The SerialPort1XonCharacter parameter + PARAMETERID_SerialPort1XonCharacter = ( (0 << 24) | 152 ), + /// \brief The SerialPort1XoffCharacter parameter + PARAMETERID_SerialPort1XoffCharacter = ( (0 << 24) | 153 ), + /// \brief The SerialPort1BaudRate parameter + PARAMETERID_SerialPort1BaudRate = ( (0 << 24) | 154 ), + /// \brief The SerialPort1Setup parameter + PARAMETERID_SerialPort1Setup = ( (0 << 24) | 155 ), + /// \brief The RequiredAxes parameter + PARAMETERID_RequiredAxes = ( (0 << 24) | 156 ), + /// \brief The JoystickInput1MinVoltage parameter + PARAMETERID_JoystickInput1MinVoltage = ( (0 << 24) | 157 ), + /// \brief The JoystickInput1MaxVoltage parameter + PARAMETERID_JoystickInput1MaxVoltage = ( (0 << 24) | 158 ), + /// \brief The JoystickInput1Deadband parameter + PARAMETERID_JoystickInput1Deadband = ( (0 << 24) | 159 ), + /// \brief The JoystickInput0MinVoltage parameter + PARAMETERID_JoystickInput0MinVoltage = ( (0 << 24) | 160 ), + /// \brief The JoystickInput0MaxVoltage parameter + PARAMETERID_JoystickInput0MaxVoltage = ( (0 << 24) | 161 ), + /// \brief The JoystickInput0Deadband parameter + PARAMETERID_JoystickInput0Deadband = ( (0 << 24) | 162 ), + /// \brief The JoystickLowSpeed parameter + PARAMETERID_JoystickLowSpeed = ( (0 << 24) | 163 ), + /// \brief The JoystickHighSpeed parameter + PARAMETERID_JoystickHighSpeed = ( (0 << 24) | 164 ), + /// \brief The JoystickSetup parameter + PARAMETERID_JoystickSetup = ( (0 << 24) | 165 ), + /// \brief The HomePositionSet parameter + PARAMETERID_HomePositionSet = ( (0 << 24) | 166 ), + /// \brief The TaskTerminationAxes parameter + PARAMETERID_TaskTerminationAxes = ( (0 << 24) | 167 ), + /// \brief The TaskStopAbortAxes parameter + PARAMETERID_TaskStopAbortAxes = ( (0 << 24) | 168 ), + /// \brief The CalibrationFile2D parameter + PARAMETERID_CalibrationFile2D = ( (0 << 24) | 169 ), + /// \brief The FaultMaskDisableDelay parameter + PARAMETERID_FaultMaskDisableDelay = ( (0 << 24) | 170 ), + /// \brief The DefaultCoordinatedSpeed parameter + PARAMETERID_DefaultCoordinatedSpeed = ( (0 << 24) | 171 ), + /// \brief The DefaultCoordinatedRampRate parameter + PARAMETERID_DefaultCoordinatedRampRate = ( (0 << 24) | 172 ), + /// \brief The DefaultDependentCoordinatedRampRate parameter + PARAMETERID_DefaultDependentCoordinatedRampRate = ( (0 << 24) | 173 ), + /// \brief The GpibTerminatingCharacter parameter + PARAMETERID_GpibTerminatingCharacter = ( (0 << 24) | 174 ), + /// \brief The GpibPrimaryAddress parameter + PARAMETERID_GpibPrimaryAddress = ( (0 << 24) | 175 ), + /// \brief The GpibParallelResponse parameter + PARAMETERID_GpibParallelResponse = ( (0 << 24) | 176 ), + /// \brief The CommandTerminatingCharacter parameter + PARAMETERID_CommandTerminatingCharacter = ( (0 << 24) | 177 ), + /// \brief The CommandSuccessCharacter parameter + PARAMETERID_CommandSuccessCharacter = ( (0 << 24) | 178 ), + /// \brief The CommandInvalidCharacter parameter + PARAMETERID_CommandInvalidCharacter = ( (0 << 24) | 179 ), + /// \brief The CommandFaultCharacter parameter + PARAMETERID_CommandFaultCharacter = ( (0 << 24) | 180 ), + /// \brief The FaultAbortAxes parameter + PARAMETERID_FaultAbortAxes = ( (0 << 24) | 182 ), + /// \brief The HarmonicCancellation0Type parameter + PARAMETERID_HarmonicCancellation0Type = ( (0 << 24) | 185 ), + /// \brief The HarmonicCancellation0Period parameter + PARAMETERID_HarmonicCancellation0Period = ( (0 << 24) | 186 ), + /// \brief The HarmonicCancellation0Gain parameter + PARAMETERID_HarmonicCancellation0Gain = ( (0 << 24) | 188 ), + /// \brief The HarmonicCancellation0Phase parameter + PARAMETERID_HarmonicCancellation0Phase = ( (0 << 24) | 189 ), + /// \brief The HarmonicCancellation1Type parameter + PARAMETERID_HarmonicCancellation1Type = ( (0 << 24) | 190 ), + /// \brief The HarmonicCancellation1Period parameter + PARAMETERID_HarmonicCancellation1Period = ( (0 << 24) | 191 ), + /// \brief The HarmonicCancellation1Gain parameter + PARAMETERID_HarmonicCancellation1Gain = ( (0 << 24) | 193 ), + /// \brief The HarmonicCancellation1Phase parameter + PARAMETERID_HarmonicCancellation1Phase = ( (0 << 24) | 194 ), + /// \brief The HarmonicCancellation2Type parameter + PARAMETERID_HarmonicCancellation2Type = ( (0 << 24) | 195 ), + /// \brief The HarmonicCancellation2Period parameter + PARAMETERID_HarmonicCancellation2Period = ( (0 << 24) | 196 ), + /// \brief The HarmonicCancellation2Gain parameter + PARAMETERID_HarmonicCancellation2Gain = ( (0 << 24) | 198 ), + /// \brief The HarmonicCancellation2Phase parameter + PARAMETERID_HarmonicCancellation2Phase = ( (0 << 24) | 199 ), + /// \brief The CommandTimeout parameter + PARAMETERID_CommandTimeout = ( (0 << 24) | 202 ), + /// \brief The CommandTimeoutCharacter parameter + PARAMETERID_CommandTimeoutCharacter = ( (0 << 24) | 203 ), + /// \brief The ResolverReferenceGain parameter + PARAMETERID_ResolverReferenceGain = ( (0 << 24) | 204 ), + /// \brief The ResolverSetup parameter + PARAMETERID_ResolverSetup = ( (0 << 24) | 205 ), + /// \brief The ResolverReferencePhase parameter + PARAMETERID_ResolverReferencePhase = ( (0 << 24) | 206 ), + /// \brief The SoftwareLimitSetup parameter + PARAMETERID_SoftwareLimitSetup = ( (0 << 24) | 210 ), + /// \brief The SSINet1Setup parameter + PARAMETERID_SSINet1Setup = ( (0 << 24) | 211 ), + /// \brief The SSINet2Setup parameter + PARAMETERID_SSINet2Setup = ( (0 << 24) | 212 ), + /// \brief The EmulatedQuadratureDivider parameter + PARAMETERID_EmulatedQuadratureDivider = ( (0 << 24) | 213 ), + /// \brief The HarmonicCancellation3Type parameter + PARAMETERID_HarmonicCancellation3Type = ( (0 << 24) | 214 ), + /// \brief The HarmonicCancellation3Period parameter + PARAMETERID_HarmonicCancellation3Period = ( (0 << 24) | 215 ), + /// \brief The HarmonicCancellation3Gain parameter + PARAMETERID_HarmonicCancellation3Gain = ( (0 << 24) | 217 ), + /// \brief The HarmonicCancellation3Phase parameter + PARAMETERID_HarmonicCancellation3Phase = ( (0 << 24) | 218 ), + /// \brief The HarmonicCancellation4Type parameter + PARAMETERID_HarmonicCancellation4Type = ( (0 << 24) | 219 ), + /// \brief The HarmonicCancellation4Period parameter + PARAMETERID_HarmonicCancellation4Period = ( (0 << 24) | 220 ), + /// \brief The HarmonicCancellation4Gain parameter + PARAMETERID_HarmonicCancellation4Gain = ( (0 << 24) | 222 ), + /// \brief The HarmonicCancellation4Phase parameter + PARAMETERID_HarmonicCancellation4Phase = ( (0 << 24) | 223 ), + /// \brief The EnhancedThroughputChannel parameter + PARAMETERID_EnhancedThroughputChannel = ( (0 << 24) | 224 ), + /// \brief The EnhancedThroughputGain parameter + PARAMETERID_EnhancedThroughputGain = ( (0 << 24) | 225 ), + /// \brief The HarmonicCancellationSetup parameter + PARAMETERID_HarmonicCancellationSetup = ( (0 << 24) | 226 ), + /// \brief The EnhancedThroughputCurrentClamp parameter + PARAMETERID_EnhancedThroughputCurrentClamp = ( (0 << 24) | 227 ), + /// \brief The Analog0Filter0CoeffN0 parameter + PARAMETERID_Analog0Filter0CoeffN0 = ( (0 << 24) | 228 ), + /// \brief The Analog0Filter0CoeffN1 parameter + PARAMETERID_Analog0Filter0CoeffN1 = ( (0 << 24) | 229 ), + /// \brief The Analog0Filter0CoeffN2 parameter + PARAMETERID_Analog0Filter0CoeffN2 = ( (0 << 24) | 230 ), + /// \brief The Analog0Filter0CoeffD1 parameter + PARAMETERID_Analog0Filter0CoeffD1 = ( (0 << 24) | 231 ), + /// \brief The Analog0Filter0CoeffD2 parameter + PARAMETERID_Analog0Filter0CoeffD2 = ( (0 << 24) | 232 ), + /// \brief The Analog0Filter1CoeffN0 parameter + PARAMETERID_Analog0Filter1CoeffN0 = ( (0 << 24) | 233 ), + /// \brief The Analog0Filter1CoeffN1 parameter + PARAMETERID_Analog0Filter1CoeffN1 = ( (0 << 24) | 234 ), + /// \brief The Analog0Filter1CoeffN2 parameter + PARAMETERID_Analog0Filter1CoeffN2 = ( (0 << 24) | 235 ), + /// \brief The Analog0Filter1CoeffD1 parameter + PARAMETERID_Analog0Filter1CoeffD1 = ( (0 << 24) | 236 ), + /// \brief The Analog0Filter1CoeffD2 parameter + PARAMETERID_Analog0Filter1CoeffD2 = ( (0 << 24) | 237 ), + /// \brief The Analog1Filter0CoeffN0 parameter + PARAMETERID_Analog1Filter0CoeffN0 = ( (0 << 24) | 238 ), + /// \brief The Analog1Filter0CoeffN1 parameter + PARAMETERID_Analog1Filter0CoeffN1 = ( (0 << 24) | 239 ), + /// \brief The Analog1Filter0CoeffN2 parameter + PARAMETERID_Analog1Filter0CoeffN2 = ( (0 << 24) | 240 ), + /// \brief The Analog1Filter0CoeffD1 parameter + PARAMETERID_Analog1Filter0CoeffD1 = ( (0 << 24) | 241 ), + /// \brief The Analog1Filter0CoeffD2 parameter + PARAMETERID_Analog1Filter0CoeffD2 = ( (0 << 24) | 242 ), + /// \brief The Analog1Filter1CoeffN0 parameter + PARAMETERID_Analog1Filter1CoeffN0 = ( (0 << 24) | 243 ), + /// \brief The Analog1Filter1CoeffN1 parameter + PARAMETERID_Analog1Filter1CoeffN1 = ( (0 << 24) | 244 ), + /// \brief The Analog1Filter1CoeffN2 parameter + PARAMETERID_Analog1Filter1CoeffN2 = ( (0 << 24) | 245 ), + /// \brief The Analog1Filter1CoeffD1 parameter + PARAMETERID_Analog1Filter1CoeffD1 = ( (0 << 24) | 246 ), + /// \brief The Analog1Filter1CoeffD2 parameter + PARAMETERID_Analog1Filter1CoeffD2 = ( (0 << 24) | 247 ), + /// \brief The GlobalStrings parameter + PARAMETERID_GlobalStrings = ( (0 << 24) | 248 ), + /// \brief The DefaultCoordinatedRampMode parameter + PARAMETERID_DefaultCoordinatedRampMode = ( (0 << 24) | 249 ), + /// \brief The DefaultCoordinatedRampTime parameter + PARAMETERID_DefaultCoordinatedRampTime = ( (0 << 24) | 250 ), + /// \brief The DefaultCoordinatedRampDistance parameter + PARAMETERID_DefaultCoordinatedRampDistance = ( (0 << 24) | 251 ), + /// \brief The DefaultRampMode parameter + PARAMETERID_DefaultRampMode = ( (0 << 24) | 252 ), + /// \brief The DefaultRampTime parameter + PARAMETERID_DefaultRampTime = ( (0 << 24) | 253 ), + /// \brief The DefaultRampDistance parameter + PARAMETERID_DefaultRampDistance = ( (0 << 24) | 254 ), + /// \brief The ServoFilterSetup parameter + PARAMETERID_ServoFilterSetup = ( (0 << 24) | 255 ), + /// \brief The FeedbackSetup parameter + PARAMETERID_FeedbackSetup = ( (0 << 24) | 256 ), + /// \brief The EncoderMultiplierSetup parameter + PARAMETERID_EncoderMultiplierSetup = ( (0 << 24) | 257 ), + /// \brief The FaultSetup parameter + PARAMETERID_FaultSetup = ( (0 << 24) | 258 ), + /// \brief The ThresholdScheduleSetup parameter + PARAMETERID_ThresholdScheduleSetup = ( (0 << 24) | 259 ), + /// \brief The ThresholdRegion2High parameter + PARAMETERID_ThresholdRegion2High = ( (0 << 24) | 260 ), + /// \brief The ThresholdRegion2Low parameter + PARAMETERID_ThresholdRegion2Low = ( (0 << 24) | 261 ), + /// \brief The ThresholdRegion3GainKpos parameter + PARAMETERID_ThresholdRegion3GainKpos = ( (0 << 24) | 262 ), + /// \brief The ThresholdRegion3GainKp parameter + PARAMETERID_ThresholdRegion3GainKp = ( (0 << 24) | 263 ), + /// \brief The ThresholdRegion3GainKi parameter + PARAMETERID_ThresholdRegion3GainKi = ( (0 << 24) | 264 ), + /// \brief The ThresholdRegion3GainKpi parameter + PARAMETERID_ThresholdRegion3GainKpi = ( (0 << 24) | 265 ), + /// \brief The ThresholdRegion4High parameter + PARAMETERID_ThresholdRegion4High = ( (0 << 24) | 266 ), + /// \brief The ThresholdRegion4Low parameter + PARAMETERID_ThresholdRegion4Low = ( (0 << 24) | 267 ), + /// \brief The ThresholdRegion5GainKpos parameter + PARAMETERID_ThresholdRegion5GainKpos = ( (0 << 24) | 268 ), + /// \brief The ThresholdRegion5GainKp parameter + PARAMETERID_ThresholdRegion5GainKp = ( (0 << 24) | 269 ), + /// \brief The ThresholdRegion5GainKi parameter + PARAMETERID_ThresholdRegion5GainKi = ( (0 << 24) | 270 ), + /// \brief The ThresholdRegion5GainKpi parameter + PARAMETERID_ThresholdRegion5GainKpi = ( (0 << 24) | 271 ), + /// \brief The DynamicScheduleSetup parameter + PARAMETERID_DynamicScheduleSetup = ( (0 << 24) | 272 ), + /// \brief The DynamicGainKposScale parameter + PARAMETERID_DynamicGainKposScale = ( (0 << 24) | 273 ), + /// \brief The DynamicGainKpScale parameter + PARAMETERID_DynamicGainKpScale = ( (0 << 24) | 274 ), + /// \brief The DynamicGainKiScale parameter + PARAMETERID_DynamicGainKiScale = ( (0 << 24) | 275 ), + /// \brief The ServoFilter4CoeffN0 parameter + PARAMETERID_ServoFilter4CoeffN0 = ( (0 << 24) | 276 ), + /// \brief The ServoFilter4CoeffN1 parameter + PARAMETERID_ServoFilter4CoeffN1 = ( (0 << 24) | 277 ), + /// \brief The ServoFilter4CoeffN2 parameter + PARAMETERID_ServoFilter4CoeffN2 = ( (0 << 24) | 278 ), + /// \brief The ServoFilter4CoeffD1 parameter + PARAMETERID_ServoFilter4CoeffD1 = ( (0 << 24) | 279 ), + /// \brief The ServoFilter4CoeffD2 parameter + PARAMETERID_ServoFilter4CoeffD2 = ( (0 << 24) | 280 ), + /// \brief The ServoFilter5CoeffN0 parameter + PARAMETERID_ServoFilter5CoeffN0 = ( (0 << 24) | 281 ), + /// \brief The ServoFilter5CoeffN1 parameter + PARAMETERID_ServoFilter5CoeffN1 = ( (0 << 24) | 282 ), + /// \brief The ServoFilter5CoeffN2 parameter + PARAMETERID_ServoFilter5CoeffN2 = ( (0 << 24) | 283 ), + /// \brief The ServoFilter5CoeffD1 parameter + PARAMETERID_ServoFilter5CoeffD1 = ( (0 << 24) | 284 ), + /// \brief The ServoFilter5CoeffD2 parameter + PARAMETERID_ServoFilter5CoeffD2 = ( (0 << 24) | 285 ), + /// \brief The ServoFilter6CoeffN0 parameter + PARAMETERID_ServoFilter6CoeffN0 = ( (0 << 24) | 286 ), + /// \brief The ServoFilter6CoeffN1 parameter + PARAMETERID_ServoFilter6CoeffN1 = ( (0 << 24) | 287 ), + /// \brief The ServoFilter6CoeffN2 parameter + PARAMETERID_ServoFilter6CoeffN2 = ( (0 << 24) | 288 ), + /// \brief The ServoFilter6CoeffD1 parameter + PARAMETERID_ServoFilter6CoeffD1 = ( (0 << 24) | 289 ), + /// \brief The ServoFilter6CoeffD2 parameter + PARAMETERID_ServoFilter6CoeffD2 = ( (0 << 24) | 290 ), + /// \brief The ServoFilter7CoeffN0 parameter + PARAMETERID_ServoFilter7CoeffN0 = ( (0 << 24) | 291 ), + /// \brief The ServoFilter7CoeffN1 parameter + PARAMETERID_ServoFilter7CoeffN1 = ( (0 << 24) | 292 ), + /// \brief The ServoFilter7CoeffN2 parameter + PARAMETERID_ServoFilter7CoeffN2 = ( (0 << 24) | 293 ), + /// \brief The ServoFilter7CoeffD1 parameter + PARAMETERID_ServoFilter7CoeffD1 = ( (0 << 24) | 294 ), + /// \brief The ServoFilter7CoeffD2 parameter + PARAMETERID_ServoFilter7CoeffD2 = ( (0 << 24) | 295 ), + /// \brief The LinearAmpMaxPower parameter + PARAMETERID_LinearAmpMaxPower = ( (0 << 24) | 296 ), + /// \brief The LinearAmpDeratingFactor parameter + PARAMETERID_LinearAmpDeratingFactor = ( (0 << 24) | 297 ), + /// \brief The LinearAmpBusVoltage parameter + PARAMETERID_LinearAmpBusVoltage = ( (0 << 24) | 298 ), + /// \brief The MotorResistance parameter + PARAMETERID_MotorResistance = ( (0 << 24) | 299 ), + /// \brief The MotorBackEMFConstant parameter + PARAMETERID_MotorBackEMFConstant = ( (0 << 24) | 300 ), + /// \brief The GantrySetup parameter + PARAMETERID_GantrySetup = ( (0 << 24) | 302 ), + /// \brief The RolloverMode parameter + PARAMETERID_RolloverMode = ( (0 << 24) | 303 ), + /// \brief The ResolverCoarseChannel parameter + PARAMETERID_ResolverCoarseChannel = ( (0 << 24) | 306 ), + /// \brief The ResolverFeedbackRatio parameter + PARAMETERID_ResolverFeedbackRatio = ( (0 << 24) | 307 ), + /// \brief The ResolverFeedbackOffset parameter + PARAMETERID_ResolverFeedbackOffset = ( (0 << 24) | 308 ), + /// \brief The InPositionTime parameter + PARAMETERID_InPositionTime = ( (0 << 24) | 319 ), + /// \brief The ExternalFaultAnalogInput parameter + PARAMETERID_ExternalFaultAnalogInput = ( (0 << 24) | 424 ), + /// \brief The ExternalFaultThreshold parameter + PARAMETERID_ExternalFaultThreshold = ( (0 << 24) | 425 ), + /// \brief The DisplayAxes parameter + PARAMETERID_DisplayAxes = ( (0 << 24) | 426 ), + /// \brief The DefaultDependentCoordinatedSpeed parameter + PARAMETERID_DefaultDependentCoordinatedSpeed = ( (0 << 24) | 427 ), + /// \brief The AnalogFilterSetup parameter + PARAMETERID_AnalogFilterSetup = ( (0 << 24) | 482 ), + /// \brief The ModbusMasterSlaveIPAddress parameter + PARAMETERID_ModbusMasterSlaveIPAddress = ( (0 << 24) | 489 ), + /// \brief The ModbusMasterSlavePort parameter + PARAMETERID_ModbusMasterSlavePort = ( (0 << 24) | 490 ), + /// \brief The ModbusMasterSlaveID parameter + PARAMETERID_ModbusMasterSlaveID = ( (0 << 24) | 491 ), + /// \brief The ModbusMasterInputWords parameter + PARAMETERID_ModbusMasterInputWords = ( (0 << 24) | 492 ), + /// \brief The ModbusMasterOutputWords parameter + PARAMETERID_ModbusMasterOutputWords = ( (0 << 24) | 493 ), + /// \brief The ModbusMasterInputBits parameter + PARAMETERID_ModbusMasterInputBits = ( (0 << 24) | 494 ), + /// \brief The ModbusMasterOutputBits parameter + PARAMETERID_ModbusMasterOutputBits = ( (0 << 24) | 495 ), + /// \brief The ModbusMasterSetup parameter + PARAMETERID_ModbusMasterSetup = ( (0 << 24) | 496 ), + /// \brief The ModbusMasterVirtualInputs parameter + PARAMETERID_ModbusMasterVirtualInputs = ( (0 << 24) | 499 ), + /// \brief The ModbusMasterVirtualOutputs parameter + PARAMETERID_ModbusMasterVirtualOutputs = ( (0 << 24) | 500 ), + /// \brief The ModbusMasterOutputWordsSections parameter + PARAMETERID_ModbusMasterOutputWordsSections = ( (0 << 24) | 501 ), + /// \brief The ModbusMasterOutputBitsSections parameter + PARAMETERID_ModbusMasterOutputBitsSections = ( (0 << 24) | 502 ), + /// \brief The ModbusMasterRWReadOffset parameter + PARAMETERID_ModbusMasterRWReadOffset = ( (0 << 24) | 503 ), + /// \brief The ModbusMasterInputWordsOffset parameter + PARAMETERID_ModbusMasterInputWordsOffset = ( (0 << 24) | 504 ), + /// \brief The ModbusMasterOutputWordsOffset parameter + PARAMETERID_ModbusMasterOutputWordsOffset = ( (0 << 24) | 505 ), + /// \brief The ModbusMasterInputBitsOffset parameter + PARAMETERID_ModbusMasterInputBitsOffset = ( (0 << 24) | 506 ), + /// \brief The ModbusMasterOutputBitsOffset parameter + PARAMETERID_ModbusMasterOutputBitsOffset = ( (0 << 24) | 507 ), + /// \brief The ModbusMasterStatusWordsOffset parameter + PARAMETERID_ModbusMasterStatusWordsOffset = ( (0 << 24) | 508 ), + /// \brief The ModbusMasterStatusBitsOffset parameter + PARAMETERID_ModbusMasterStatusBitsOffset = ( (0 << 24) | 509 ), + /// \brief The ModbusMasterVirtualInputsOffset parameter + PARAMETERID_ModbusMasterVirtualInputsOffset = ( (0 << 24) | 510 ), + /// \brief The ModbusMasterVirtualOutputsOffset parameter + PARAMETERID_ModbusMasterVirtualOutputsOffset = ( (0 << 24) | 511 ), + /// \brief The ModbusMasterRWWriteOffset parameter + PARAMETERID_ModbusMasterRWWriteOffset = ( (0 << 24) | 512 ), + /// \brief The ModbusMasterFunctions parameter + PARAMETERID_ModbusMasterFunctions = ( (0 << 24) | 513 ), + /// \brief The ModbusMasterSlaveType parameter + PARAMETERID_ModbusMasterSlaveType = ( (0 << 24) | 514 ), + /// \brief The ModbusSlaveUnitID parameter + PARAMETERID_ModbusSlaveUnitID = ( (0 << 24) | 516 ), + /// \brief The ModbusSlaveInputWords parameter + PARAMETERID_ModbusSlaveInputWords = ( (0 << 24) | 517 ), + /// \brief The ModbusSlaveOutputWords parameter + PARAMETERID_ModbusSlaveOutputWords = ( (0 << 24) | 518 ), + /// \brief The ModbusSlaveInputBits parameter + PARAMETERID_ModbusSlaveInputBits = ( (0 << 24) | 519 ), + /// \brief The ModbusSlaveOutputBits parameter + PARAMETERID_ModbusSlaveOutputBits = ( (0 << 24) | 520 ), + /// \brief The ModbusSlaveInputWordsOffset parameter + PARAMETERID_ModbusSlaveInputWordsOffset = ( (0 << 24) | 521 ), + /// \brief The ModbusSlaveOutputWordsOffset parameter + PARAMETERID_ModbusSlaveOutputWordsOffset = ( (0 << 24) | 522 ), + /// \brief The ModbusSlaveInputBitsOffset parameter + PARAMETERID_ModbusSlaveInputBitsOffset = ( (0 << 24) | 523 ), + /// \brief The ModbusSlaveOutputBitsOffset parameter + PARAMETERID_ModbusSlaveOutputBitsOffset = ( (0 << 24) | 524 ), + /// \brief The ModbusSlaveRWReadOffset parameter + PARAMETERID_ModbusSlaveRWReadOffset = ( (0 << 24) | 525 ), + /// \brief The ModbusSlaveRWWriteOffset parameter + PARAMETERID_ModbusSlaveRWWriteOffset = ( (0 << 24) | 526 ), + /// \brief The CurrentOffsetA parameter + PARAMETERID_CurrentOffsetA = ( (0 << 24) | 662 ), + /// \brief The CurrentOffsetB parameter + PARAMETERID_CurrentOffsetB = ( (0 << 24) | 663 ), + /// \brief The CommandShaperSetup parameter + PARAMETERID_CommandShaperSetup = ( (0 << 24) | 666 ), + /// \brief The CommandShaperTime00 parameter + PARAMETERID_CommandShaperTime00 = ( (0 << 24) | 667 ), + /// \brief The CommandShaperTime01 parameter + PARAMETERID_CommandShaperTime01 = ( (0 << 24) | 668 ), + /// \brief The CommandShaperTime02 parameter + PARAMETERID_CommandShaperTime02 = ( (0 << 24) | 669 ), + /// \brief The CommandShaperTime03 parameter + PARAMETERID_CommandShaperTime03 = ( (0 << 24) | 670 ), + /// \brief The CommandShaperTime04 parameter + PARAMETERID_CommandShaperTime04 = ( (0 << 24) | 671 ), + /// \brief The CommandShaperTime05 parameter + PARAMETERID_CommandShaperTime05 = ( (0 << 24) | 672 ), + /// \brief The CommandShaperTime06 parameter + PARAMETERID_CommandShaperTime06 = ( (0 << 24) | 673 ), + /// \brief The CommandShaperTime07 parameter + PARAMETERID_CommandShaperTime07 = ( (0 << 24) | 674 ), + /// \brief The CommandShaperTime08 parameter + PARAMETERID_CommandShaperTime08 = ( (0 << 24) | 675 ), + /// \brief The CommandShaperTime09 parameter + PARAMETERID_CommandShaperTime09 = ( (0 << 24) | 676 ), + /// \brief The CommandShaperTime10 parameter + PARAMETERID_CommandShaperTime10 = ( (0 << 24) | 677 ), + /// \brief The CommandShaperTime11 parameter + PARAMETERID_CommandShaperTime11 = ( (0 << 24) | 678 ), + /// \brief The CommandShaperTime12 parameter + PARAMETERID_CommandShaperTime12 = ( (0 << 24) | 679 ), + /// \brief The CommandShaperTime13 parameter + PARAMETERID_CommandShaperTime13 = ( (0 << 24) | 680 ), + /// \brief The CommandShaperTime14 parameter + PARAMETERID_CommandShaperTime14 = ( (0 << 24) | 681 ), + /// \brief The CommandShaperTime15 parameter + PARAMETERID_CommandShaperTime15 = ( (0 << 24) | 682 ), + /// \brief The CommandShaperCoeff00 parameter + PARAMETERID_CommandShaperCoeff00 = ( (0 << 24) | 683 ), + /// \brief The CommandShaperCoeff01 parameter + PARAMETERID_CommandShaperCoeff01 = ( (0 << 24) | 684 ), + /// \brief The CommandShaperCoeff02 parameter + PARAMETERID_CommandShaperCoeff02 = ( (0 << 24) | 685 ), + /// \brief The CommandShaperCoeff03 parameter + PARAMETERID_CommandShaperCoeff03 = ( (0 << 24) | 686 ), + /// \brief The CommandShaperCoeff04 parameter + PARAMETERID_CommandShaperCoeff04 = ( (0 << 24) | 687 ), + /// \brief The CommandShaperCoeff05 parameter + PARAMETERID_CommandShaperCoeff05 = ( (0 << 24) | 688 ), + /// \brief The CommandShaperCoeff06 parameter + PARAMETERID_CommandShaperCoeff06 = ( (0 << 24) | 689 ), + /// \brief The CommandShaperCoeff07 parameter + PARAMETERID_CommandShaperCoeff07 = ( (0 << 24) | 690 ), + /// \brief The CommandShaperCoeff08 parameter + PARAMETERID_CommandShaperCoeff08 = ( (0 << 24) | 691 ), + /// \brief The CommandShaperCoeff09 parameter + PARAMETERID_CommandShaperCoeff09 = ( (0 << 24) | 692 ), + /// \brief The CommandShaperCoeff10 parameter + PARAMETERID_CommandShaperCoeff10 = ( (0 << 24) | 693 ), + /// \brief The CommandShaperCoeff11 parameter + PARAMETERID_CommandShaperCoeff11 = ( (0 << 24) | 694 ), + /// \brief The CommandShaperCoeff12 parameter + PARAMETERID_CommandShaperCoeff12 = ( (0 << 24) | 695 ), + /// \brief The CommandShaperCoeff13 parameter + PARAMETERID_CommandShaperCoeff13 = ( (0 << 24) | 696 ), + /// \brief The CommandShaperCoeff14 parameter + PARAMETERID_CommandShaperCoeff14 = ( (0 << 24) | 697 ), + /// \brief The CommandShaperCoeff15 parameter + PARAMETERID_CommandShaperCoeff15 = ( (0 << 24) | 698 ), + /// \brief The CommandShaper0Type parameter + PARAMETERID_CommandShaper0Type = ( (0 << 24) | 703 ), + /// \brief The CommandShaper0Frequency parameter + PARAMETERID_CommandShaper0Frequency = ( (0 << 24) | 704 ), + /// \brief The CommandShaper0Damping parameter + PARAMETERID_CommandShaper0Damping = ( (0 << 24) | 705 ), + /// \brief The CommandShaper1Type parameter + PARAMETERID_CommandShaper1Type = ( (0 << 24) | 706 ), + /// \brief The CommandShaper1Frequency parameter + PARAMETERID_CommandShaper1Frequency = ( (0 << 24) | 707 ), + /// \brief The CommandShaper1Damping parameter + PARAMETERID_CommandShaper1Damping = ( (0 << 24) | 708 ), + /// \brief The ResoluteEncoderSetup parameter + PARAMETERID_ResoluteEncoderSetup = ( (0 << 24) | 715 ), + /// \brief The ResoluteEncoderResolution parameter + PARAMETERID_ResoluteEncoderResolution = ( (0 << 24) | 716 ), + /// \brief The ResoluteEncoderUserResolution parameter + PARAMETERID_ResoluteEncoderUserResolution = ( (0 << 24) | 717 ), + /// \brief The AutofocusInput parameter + PARAMETERID_AutofocusInput = ( (0 << 24) | 721 ), + /// \brief The AutofocusTarget parameter + PARAMETERID_AutofocusTarget = ( (0 << 24) | 722 ), + /// \brief The AutofocusDeadband parameter + PARAMETERID_AutofocusDeadband = ( (0 << 24) | 723 ), + /// \brief The AutofocusGainKi parameter + PARAMETERID_AutofocusGainKi = ( (0 << 24) | 724 ), + /// \brief The AutofocusGainKp parameter + PARAMETERID_AutofocusGainKp = ( (0 << 24) | 725 ), + /// \brief The AutofocusLimitLow parameter + PARAMETERID_AutofocusLimitLow = ( (0 << 24) | 726 ), + /// \brief The AutofocusLimitHigh parameter + PARAMETERID_AutofocusLimitHigh = ( (0 << 24) | 727 ), + /// \brief The AutofocusSpeedClamp parameter + PARAMETERID_AutofocusSpeedClamp = ( (0 << 24) | 728 ), + /// \brief The AutofocusHoldInput parameter + PARAMETERID_AutofocusHoldInput = ( (0 << 24) | 729 ), + /// \brief The AutofocusSetup parameter + PARAMETERID_AutofocusSetup = ( (0 << 24) | 730 ), + /// \brief The ExternalSyncFrequency parameter + PARAMETERID_ExternalSyncFrequency = ( (0 << 24) | 731 ), + /// \brief The GainPff parameter + PARAMETERID_GainPff = ( (0 << 24) | 762 ), + /// \brief The AutofocusInitialRampTime parameter + PARAMETERID_AutofocusInitialRampTime = ( (0 << 24) | 763 ), +} PARAMETERID; + +#endif // __PARAMETER_ID_H__ diff --git a/motorApp/AerotechSrc/doCommand.ab b/motorApp/AerotechSrc/doCommand.ab index 887996934..e459bc51e 100755 --- a/motorApp/AerotechSrc/doCommand.ab +++ b/motorApp/AerotechSrc/doCommand.ab @@ -1,235 +1,235 @@ -HEADER - DEFINE cmdDONE 0 - DEFINE cmdVELOCITY_ON 1 - DEFINE cmdVELOCITY_OFF 2 - DEFINE cmdHALT 3 - DEFINE cmdSTART 4 - DEFINE cmdPVT_INIT_TIME_ABS 5 - DEFINE cmdPVT_INIT_TIME_INC 6 - DEFINE cmdPVT1 7 - DEFINE cmdPVT2 8 - DEFINE cmdPVT3 9 - DEFINE cmdPVT 10 - DEFINE cmdABORT 11 - DEFINE cmdSTARTABORT 12 - - DEFINE cmdSCOPEBUFFER 13 - DEFINE cmdSCOPEDATA 14 - DEFINE cmdSCOPESTATUS 15 - DEFINE cmdSCOPETRIG 16 - DEFINE cmdSCOPETRIGPERIOD 17 - DEFINE cmdDRIVEINFO 18 - DEFINE cmdLINEAR 19 - DEFINE cmdDATAACQ_TRIG 20 - DEFINE cmdDATAACQ_INP 21 - DEFINE cmdDATAACQ_ON 22 - DEFINE cmdDATAACQ_OFF 23 - DEFINE cmdDATAACQ_READ 24 - DEFINE cmdDOTRAJECTORY 25 - - DEFINE cmdVar 45 - DEFINE iarg1Var 46 - DEFINE iarg2Var 47 - DEFINE iarg3Var 48 - DEFINE iarg4Var 49 - DEFINE darg1Var 1 - DEFINE darg2Var 2 - DEFINE darg3Var 3 - DEFINE darg4Var 4 - DEFINE darg5Var 5 - DEFINE darg6Var 6 - DEFINE darg7Var 7 - DEFINE darg8Var 8 - DEFINE darg9Var 9 - DEFINE numIArg 44 - DEFINE numDArg 43 - DEFINE pvtWaitMSVar 42 - - ' Numerical values for first arg to scopedata() - DEFINE sd_PositionCommand 0 - DEFINE sd_PositionFeedback 1 - DEFINE sd_ExternalPosition 2 - DEFINE sd_AxisFault 3 - DEFINE sd_AxisStatus 4 - DEFINE sd_AnalogInput0 5 - DEFINE sd_AnalogInput1 6 - DEFINE sd_AnalogOutput0 7 - DEFINE sd_AnalogOutput1 8 - DEFINE sd_DigitalInput0 9 - DEFINE sd_DigitalInput1 10 - DEFINE sd_DigitalOutput0 11 - DEFINE sd_DigitalOutput1 12 - DEFINE sd_CurrentCommand 13 - DEFINE sd_CurrentFeedback 14 - DEFINE sd_OptionalData1 15 - DEFINE sd_OptionalData2 16 - DEFINE sd_ProgramCounter 17 - -END HEADER - -PROGRAM - - DIM axis1Number AS Integer - DIM axis2Number AS Integer - DIM axis3Number AS Integer - DIM axis4Number AS Integer - DIM keepon as Integer - DIM iarg as integer - DIM pvtWaitMS as integer - dim pos as double - dim vel as double - dim timeval as double - dim timeIsAbs as integer - dim timeInitialized as integer - dim numpoints as integer - dim i as integer - - wait mode nowait - ABS - keepon = 1 - pvtWaitMS = IGLOBAL(pvtWaitMSVar) - IF pvtWaitMS < 1 THEN - pvtWaitMS = 50 - END IF - WHILE keepon - IF IGLOBAL(cmdVar) = cmdPVT1 THEN - axis1Number = IGLOBAL(iarg1Var) - pos = DGLOBAL(darg1Var) - vel = DGLOBAL(darg2Var) - timeval = DGLOBAL(darg3Var) - if timeIsAbs and not timeInitialized then - PVT INIT TIME ABS - timeInitialized = 1 - end if - PVT @axis1Number pos, vel TIME timeval - ELSEIF IGLOBAL(cmdVar) = cmdVELOCITY_ON THEN - VELOCITY ON - ELSEIF IGLOBAL(cmdVar) = cmdVELOCITY_OFF THEN - VELOCITY OFF - ELSEIF IGLOBAL(cmdVar) = cmdHALT THEN - HALT - ELSEIF IGLOBAL(cmdVar) = cmdSTARTABORT THEN - START - ABORT @0 - acknowledgeall - enable @0 - keepon = 0 - ELSEIF IGLOBAL(cmdVar) = cmdSTART THEN - START - keepon = 0 - ELSEIF IGLOBAL(cmdVar) = cmdPVT THEN - axis1Number = IGLOBAL(iarg1Var) - axis2Number = IGLOBAL(iarg2Var) - axis3Number = IGLOBAL(iarg3Var) - axis4Number = IGLOBAL(iarg4Var) - if IGLOBAL(numIArg) = 1 then - PVT @axis1Number DGLOBAL(darg1Var), DGLOBAL(darg2Var) TIME DGLOBAL(darg3Var) - elseif IGLOBAL(numIArg) = 2 then - PVT @axis1Number DGLOBAL(darg1Var), DGLOBAL(darg2Var) @axis2Number DGLOBAL(darg3Var), DGLOBAL(darg4Var) TIME DGLOBAL(darg5Var) - elseif IGLOBAL(numIArg) = 3 then - PVT @axis1Number DGLOBAL(darg1Var), DGLOBAL(darg2Var) @axis2Number DGLOBAL(darg3Var), DGLOBAL(darg4Var) @axis3Number DGLOBAL(darg5Var), DGLOBAL(darg6Var) TIME DGLOBAL(darg7Var) - elseif IGLOBAL(numIArg) = 4 then - PVT @axis1Number DGLOBAL(darg1Var), DGLOBAL(darg2Var) @axis2Number DGLOBAL(darg3Var), DGLOBAL(darg4Var) @axis3Number DGLOBAL(darg5Var), DGLOBAL(darg6Var) @axis4Number DGLOBAL(darg7Var), DGLOBAL(darg8Var) TIME DGLOBAL(darg9Var) - end if - ELSEIF IGLOBAL(cmdVar) = cmdPVT_INIT_TIME_ABS THEN - 'PVT INIT TIME ABS - timeIsAbs = 1 - timeInitialized = 0 - ELSEIF IGLOBAL(cmdVar) = cmdPVT_INIT_TIME_INC THEN - timeIsAbs = 0 - PVT INIT TIME INC - ELSEIF IGLOBAL(cmdVar) = cmdABORT THEN - axis1Number = IGLOBAL(iarg1Var) - axis2Number = IGLOBAL(iarg2Var) - axis3Number = IGLOBAL(iarg3Var) - axis4Number = IGLOBAL(iarg4Var) - if IGLOBAL(numIArg) = 1 then - ABORT @axis1Number - elseif IGLOBAL(numIArg) = 2 then - ABORT @axis1Number @axis2Number - elseif IGLOBAL(numIArg) = 3 then - ABORT @axis1Number @axis2Number @axis3Number - elseif IGLOBAL(numIArg) = 4 then - ABORT @axis1Number @axis2Number @axis3Number @axis4Number - end if - keepon = 0 - ELSEIF IGLOBAL(cmdVar) = cmdSCOPEBUFFER THEN - iarg = IGLOBAL(iarg1Var) - SCOPEBUFFER iarg - ELSEIF IGLOBAL(cmdVar) = cmdSCOPEDATA THEN - iarg = IGLOBAL(iarg1Var) - if iarg = sd_PositionCommand then - DGLOBAL(darg1Var) = SCOPEDATA(PositionCommand, IGLOBAL(iarg2Var)) - elseif iarg = sd_PositionFeedback then - DGLOBAL(darg1Var) = SCOPEDATA(PositionFeedback, IGLOBAL(iarg2Var)) - elseif iarg = sd_CurrentFeedback then - DGLOBAL(darg1Var) = SCOPEDATA(CurrentFeedback, IGLOBAL(iarg2Var)) - else - DGLOBAL(darg1Var) = -1.2345654321 - end if - ELSEIF IGLOBAL(cmdVar) = cmdSCOPESTATUS THEN - IGLOBAL(iarg1Var) = SCOPESTATUS(IGLOBAL(iarg1Var)) - ELSEIF IGLOBAL(cmdVar) = cmdSCOPETRIG THEN - if IGLOBAL(iarg1Var) = 1 then - SCOPETRIG STOP - ELSE - SCOPETRIG - END IF - ELSEIF IGLOBAL(cmdVar) = cmdSCOPETRIGPERIOD THEN - iarg = IGLOBAL(iarg1Var) - 'SCOPETRIGPERIOD iarg - SCOPETRIGPERIOD iarg*1.0 - ' Not until version 4.03 - ' ELSEIF IGLOBAL(cmdVar) = cmdDRIVEINFO THEN - ' IGLOBAL(iarg1Var) = DRIVEINFO(X, 59) - ELSEIF IGLOBAL(cmdVar) = cmdLINEAR THEN - axis1Number = IGLOBAL(iarg1Var) - LINEAR @axis1Number DGLOBAL(darg1Var) F DGLOBAL(darg2Var) - ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_TRIG THEN - axis1Number = IGLOBAL(iarg1Var) - DATAACQ @axis1Number TRIGGER IGLOBAL(iarg2Var) - ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_INP THEN - axis1Number = IGLOBAL(iarg1Var) - DATAACQ @axis1Number INPUT IGLOBAL(iarg2Var) - ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_ON THEN - axis1Number = IGLOBAL(iarg1Var) - DATAACQ @axis1Number ON IGLOBAL(iarg2Var) - ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_OFF THEN - axis1Number = IGLOBAL(iarg1Var) - DATAACQ @axis1Number OFF - ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_READ THEN - axis1Number = IGLOBAL(iarg1Var) - DATAACQ @axis1Number READ IGLOBAL(iarg2Var), IGLOBAL(iarg3Var) - ELSEIF IGLOBAL(cmdVar) = cmdDOTRAJECTORY THEN - axis1Number = IGLOBAL(iarg1Var) - numpoints = IGLOBAL(iarg2Var) - VELOCITY ON - PVT INIT TIME ABS - for i = 0 to numpoints-1 step 1 - pos = dglobal(3*i) - vel = dglobal(3*i+1) - timeVal = dglobal(3*i+2) - if i = numpoints-2 then - VELOCITY OFF - end if - PVT @axis1Number pos, vel time timeVal - dwell pvtWaitMS/1000 - next i - elseif IGLOBAL(cmdVar) = cmdDONE then - ' do nothing - ' else - ' IGLOBAL(cmdVar) = 1000 - END IF - - IF IGLOBAL(cmdVar) = cmdPVT1 THEN - D = pvtWaitMS/1000 - ELSE - D = .001 - END IF - if IGLOBAL(cmdVar) > 0 then - IGLOBAL(cmdVar) = -(IGLOBAL(cmdVar)) - end if - DWELL D - WEND - -END PROGRAM +HEADER + DEFINE cmdDONE 0 + DEFINE cmdVELOCITY_ON 1 + DEFINE cmdVELOCITY_OFF 2 + DEFINE cmdHALT 3 + DEFINE cmdSTART 4 + DEFINE cmdPVT_INIT_TIME_ABS 5 + DEFINE cmdPVT_INIT_TIME_INC 6 + DEFINE cmdPVT1 7 + DEFINE cmdPVT2 8 + DEFINE cmdPVT3 9 + DEFINE cmdPVT 10 + DEFINE cmdABORT 11 + DEFINE cmdSTARTABORT 12 + + DEFINE cmdSCOPEBUFFER 13 + DEFINE cmdSCOPEDATA 14 + DEFINE cmdSCOPESTATUS 15 + DEFINE cmdSCOPETRIG 16 + DEFINE cmdSCOPETRIGPERIOD 17 + DEFINE cmdDRIVEINFO 18 + DEFINE cmdLINEAR 19 + DEFINE cmdDATAACQ_TRIG 20 + DEFINE cmdDATAACQ_INP 21 + DEFINE cmdDATAACQ_ON 22 + DEFINE cmdDATAACQ_OFF 23 + DEFINE cmdDATAACQ_READ 24 + DEFINE cmdDOTRAJECTORY 25 + + DEFINE cmdVar 45 + DEFINE iarg1Var 46 + DEFINE iarg2Var 47 + DEFINE iarg3Var 48 + DEFINE iarg4Var 49 + DEFINE darg1Var 1 + DEFINE darg2Var 2 + DEFINE darg3Var 3 + DEFINE darg4Var 4 + DEFINE darg5Var 5 + DEFINE darg6Var 6 + DEFINE darg7Var 7 + DEFINE darg8Var 8 + DEFINE darg9Var 9 + DEFINE numIArg 44 + DEFINE numDArg 43 + DEFINE pvtWaitMSVar 42 + + ' Numerical values for first arg to scopedata() + DEFINE sd_PositionCommand 0 + DEFINE sd_PositionFeedback 1 + DEFINE sd_ExternalPosition 2 + DEFINE sd_AxisFault 3 + DEFINE sd_AxisStatus 4 + DEFINE sd_AnalogInput0 5 + DEFINE sd_AnalogInput1 6 + DEFINE sd_AnalogOutput0 7 + DEFINE sd_AnalogOutput1 8 + DEFINE sd_DigitalInput0 9 + DEFINE sd_DigitalInput1 10 + DEFINE sd_DigitalOutput0 11 + DEFINE sd_DigitalOutput1 12 + DEFINE sd_CurrentCommand 13 + DEFINE sd_CurrentFeedback 14 + DEFINE sd_OptionalData1 15 + DEFINE sd_OptionalData2 16 + DEFINE sd_ProgramCounter 17 + +END HEADER + +PROGRAM + + DIM axis1Number AS Integer + DIM axis2Number AS Integer + DIM axis3Number AS Integer + DIM axis4Number AS Integer + DIM keepon as Integer + DIM iarg as integer + DIM pvtWaitMS as integer + dim pos as double + dim vel as double + dim timeval as double + dim timeIsAbs as integer + dim timeInitialized as integer + dim numpoints as integer + dim i as integer + + wait mode nowait + ABS + keepon = 1 + pvtWaitMS = IGLOBAL(pvtWaitMSVar) + IF pvtWaitMS < 1 THEN + pvtWaitMS = 50 + END IF + WHILE keepon + IF IGLOBAL(cmdVar) = cmdPVT1 THEN + axis1Number = IGLOBAL(iarg1Var) + pos = DGLOBAL(darg1Var) + vel = DGLOBAL(darg2Var) + timeval = DGLOBAL(darg3Var) + if timeIsAbs and not timeInitialized then + PVT INIT TIME ABS + timeInitialized = 1 + end if + PVT @axis1Number pos, vel TIME timeval + ELSEIF IGLOBAL(cmdVar) = cmdVELOCITY_ON THEN + VELOCITY ON + ELSEIF IGLOBAL(cmdVar) = cmdVELOCITY_OFF THEN + VELOCITY OFF + ELSEIF IGLOBAL(cmdVar) = cmdHALT THEN + HALT + ELSEIF IGLOBAL(cmdVar) = cmdSTARTABORT THEN + START + ABORT @0 + acknowledgeall + enable @0 + keepon = 0 + ELSEIF IGLOBAL(cmdVar) = cmdSTART THEN + START + keepon = 0 + ELSEIF IGLOBAL(cmdVar) = cmdPVT THEN + axis1Number = IGLOBAL(iarg1Var) + axis2Number = IGLOBAL(iarg2Var) + axis3Number = IGLOBAL(iarg3Var) + axis4Number = IGLOBAL(iarg4Var) + if IGLOBAL(numIArg) = 1 then + PVT @axis1Number DGLOBAL(darg1Var), DGLOBAL(darg2Var) TIME DGLOBAL(darg3Var) + elseif IGLOBAL(numIArg) = 2 then + PVT @axis1Number DGLOBAL(darg1Var), DGLOBAL(darg2Var) @axis2Number DGLOBAL(darg3Var), DGLOBAL(darg4Var) TIME DGLOBAL(darg5Var) + elseif IGLOBAL(numIArg) = 3 then + PVT @axis1Number DGLOBAL(darg1Var), DGLOBAL(darg2Var) @axis2Number DGLOBAL(darg3Var), DGLOBAL(darg4Var) @axis3Number DGLOBAL(darg5Var), DGLOBAL(darg6Var) TIME DGLOBAL(darg7Var) + elseif IGLOBAL(numIArg) = 4 then + PVT @axis1Number DGLOBAL(darg1Var), DGLOBAL(darg2Var) @axis2Number DGLOBAL(darg3Var), DGLOBAL(darg4Var) @axis3Number DGLOBAL(darg5Var), DGLOBAL(darg6Var) @axis4Number DGLOBAL(darg7Var), DGLOBAL(darg8Var) TIME DGLOBAL(darg9Var) + end if + ELSEIF IGLOBAL(cmdVar) = cmdPVT_INIT_TIME_ABS THEN + 'PVT INIT TIME ABS + timeIsAbs = 1 + timeInitialized = 0 + ELSEIF IGLOBAL(cmdVar) = cmdPVT_INIT_TIME_INC THEN + timeIsAbs = 0 + PVT INIT TIME INC + ELSEIF IGLOBAL(cmdVar) = cmdABORT THEN + axis1Number = IGLOBAL(iarg1Var) + axis2Number = IGLOBAL(iarg2Var) + axis3Number = IGLOBAL(iarg3Var) + axis4Number = IGLOBAL(iarg4Var) + if IGLOBAL(numIArg) = 1 then + ABORT @axis1Number + elseif IGLOBAL(numIArg) = 2 then + ABORT @axis1Number @axis2Number + elseif IGLOBAL(numIArg) = 3 then + ABORT @axis1Number @axis2Number @axis3Number + elseif IGLOBAL(numIArg) = 4 then + ABORT @axis1Number @axis2Number @axis3Number @axis4Number + end if + keepon = 0 + ELSEIF IGLOBAL(cmdVar) = cmdSCOPEBUFFER THEN + iarg = IGLOBAL(iarg1Var) + SCOPEBUFFER iarg + ELSEIF IGLOBAL(cmdVar) = cmdSCOPEDATA THEN + iarg = IGLOBAL(iarg1Var) + if iarg = sd_PositionCommand then + DGLOBAL(darg1Var) = SCOPEDATA(PositionCommand, IGLOBAL(iarg2Var)) + elseif iarg = sd_PositionFeedback then + DGLOBAL(darg1Var) = SCOPEDATA(PositionFeedback, IGLOBAL(iarg2Var)) + elseif iarg = sd_CurrentFeedback then + DGLOBAL(darg1Var) = SCOPEDATA(CurrentFeedback, IGLOBAL(iarg2Var)) + else + DGLOBAL(darg1Var) = -1.2345654321 + end if + ELSEIF IGLOBAL(cmdVar) = cmdSCOPESTATUS THEN + IGLOBAL(iarg1Var) = SCOPESTATUS(IGLOBAL(iarg1Var)) + ELSEIF IGLOBAL(cmdVar) = cmdSCOPETRIG THEN + if IGLOBAL(iarg1Var) = 1 then + SCOPETRIG STOP + ELSE + SCOPETRIG + END IF + ELSEIF IGLOBAL(cmdVar) = cmdSCOPETRIGPERIOD THEN + iarg = IGLOBAL(iarg1Var) + 'SCOPETRIGPERIOD iarg + SCOPETRIGPERIOD iarg*1.0 + ' Not until version 4.03 + ' ELSEIF IGLOBAL(cmdVar) = cmdDRIVEINFO THEN + ' IGLOBAL(iarg1Var) = DRIVEINFO(X, 59) + ELSEIF IGLOBAL(cmdVar) = cmdLINEAR THEN + axis1Number = IGLOBAL(iarg1Var) + LINEAR @axis1Number DGLOBAL(darg1Var) F DGLOBAL(darg2Var) + ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_TRIG THEN + axis1Number = IGLOBAL(iarg1Var) + DATAACQ @axis1Number TRIGGER IGLOBAL(iarg2Var) + ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_INP THEN + axis1Number = IGLOBAL(iarg1Var) + DATAACQ @axis1Number INPUT IGLOBAL(iarg2Var) + ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_ON THEN + axis1Number = IGLOBAL(iarg1Var) + DATAACQ @axis1Number ON IGLOBAL(iarg2Var) + ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_OFF THEN + axis1Number = IGLOBAL(iarg1Var) + DATAACQ @axis1Number OFF + ELSEIF IGLOBAL(cmdVar) = cmdDATAACQ_READ THEN + axis1Number = IGLOBAL(iarg1Var) + DATAACQ @axis1Number READ IGLOBAL(iarg2Var), IGLOBAL(iarg3Var) + ELSEIF IGLOBAL(cmdVar) = cmdDOTRAJECTORY THEN + axis1Number = IGLOBAL(iarg1Var) + numpoints = IGLOBAL(iarg2Var) + VELOCITY ON + PVT INIT TIME ABS + for i = 0 to numpoints-1 step 1 + pos = dglobal(3*i) + vel = dglobal(3*i+1) + timeVal = dglobal(3*i+2) + if i = numpoints-2 then + VELOCITY OFF + end if + PVT @axis1Number pos, vel time timeVal + dwell pvtWaitMS/1000 + next i + elseif IGLOBAL(cmdVar) = cmdDONE then + ' do nothing + ' else + ' IGLOBAL(cmdVar) = 1000 + END IF + + IF IGLOBAL(cmdVar) = cmdPVT1 THEN + D = pvtWaitMS/1000 + ELSE + D = .001 + END IF + if IGLOBAL(cmdVar) > 0 then + IGLOBAL(cmdVar) = -(IGLOBAL(cmdVar)) + end if + DWELL D + WEND + +END PROGRAM diff --git a/motorApp/HytecSrc/README_Hy8601Asyn b/motorApp/HytecSrc/README_Hy8601Asyn index e2c396229..47445d623 100644 --- a/motorApp/HytecSrc/README_Hy8601Asyn +++ b/motorApp/HytecSrc/README_Hy8601Asyn @@ -37,53 +37,53 @@ Support modules Hytec 8002/8003/8004 carrier card configuration =============================================== - -This board is a 6U VME64x carrier card. It provides four single size IP slots and -is configurable for many parameters. It supports interrupt level from 0 to 7. Any -IP cards can be enabled/disabled interrupt. The clock can be set to either 8MHz or -32MHz. For IPAC Memory space if required, the base address can be defined automatically -by geographical addressing when VME64x crate is used. For other type of crates, -the base address can be set by configuring a series of jumpers on the board or by -passing parameter to the memory offset register in the ipacAddCarrier call. - -The IPAC Carrier Driver for this board is found in the file drvHy8002.c which -implements two commands ipacAddHy8002 and Hy8002CarrierInfo. The ipacAddHy8002 -command is used to add a Hytec 8002/8003 board to the system. The Hy8002CarrierInfo -reports hardware information for a specified board or all boards in the system if -the parameter passed down is 0xFFFF. These commands are registered by the registrar -routine Hy8002Registrar to add them to the iocsh and link the driver into a final -IOC executable, for which it must be listed in the IOC's .dbd file thus: - - registrar(Hy8002Registrar) - - + +This board is a 6U VME64x carrier card. It provides four single size IP slots and +is configurable for many parameters. It supports interrupt level from 0 to 7. Any +IP cards can be enabled/disabled interrupt. The clock can be set to either 8MHz or +32MHz. For IPAC Memory space if required, the base address can be defined automatically +by geographical addressing when VME64x crate is used. For other type of crates, +the base address can be set by configuring a series of jumpers on the board or by +passing parameter to the memory offset register in the ipacAddCarrier call. + +The IPAC Carrier Driver for this board is found in the file drvHy8002.c which +implements two commands ipacAddHy8002 and Hy8002CarrierInfo. The ipacAddHy8002 +command is used to add a Hytec 8002/8003 board to the system. The Hy8002CarrierInfo +reports hardware information for a specified board or all boards in the system if +the parameter passed down is 0xFFFF. These commands are registered by the registrar +routine Hy8002Registrar to add them to the iocsh and link the driver into a final +IOC executable, for which it must be listed in the IOC's .dbd file thus: + + registrar(Hy8002Registrar) + + 1). Configuration Command and Parameter --------------------------------------- - -- int ipacAddHYy8002(const char *cardParams); - -The parameter string should comprise two (2) to six (6) parameters which are comma -separated. The first two are mandate and have to be separated only by one comma. +-------------------------------------- + +- int ipacAddHYy8002(const char *cardParams); + +The parameter string should comprise two (2) to six (6) parameters which are comma +separated. The first two are mandate and have to be separated only by one comma. The others are key/value pairs and are optional as per the conditions of the operating -system used. The format is defined as - -s,i,IPMEM=d,IPCLCK=d,ROAK=d,MEMOFFS=d - -where d is a decimal integer number. +system used. The format is defined as + +s,i,IPMEM=d,IPCLCK=d,ROAK=d,MEMOFFS=d + +where d is a decimal integer number. s defines the VME slot number of the carrier card. Valid number is 2 ~ 21. this number MUST be the same as the VME slot number where the carrier - card is plugged in. -i defines the interrupt level. Valid number is 0 ~ 7. -IPMEM=d defines the maximum memory size of the IP module. The valid values are 1, 2, + card is plugged in. +i defines the interrupt level. Valid number is 0 ~ 7. +IPMEM=d defines the maximum memory size of the IP module. The valid values are 1, 2, 4 or 8 that represent 1MB, 2MB, 4MB or 8MB respectively. Default is 1. For majority Hytec ADCs, DACs such as 8401, 8414, 8417, 8402 and 8415 etc, they all have 2MB on board. The user can choose to use either 1MB or 2MB. None of - the Hytec IPs has more than 2MB on board memory. Other vendors might have. -IPCLCK=d defines the clock that its value has to be either 8 for 8MHz or 32 for - 32Mhz. Default is 8. -ROAK=d if d =1, it defines carrier card to release the interrupt upon the - acknowledgment. If d=0, the interrupt is released by user interrupt - service routine. Default is 0. + the Hytec IPs has more than 2MB on board memory. Other vendors might have. +IPCLCK=d defines the clock that its value has to be either 8 for 8MHz or 32 for + 32Mhz. Default is 8. +ROAK=d if d =1, it defines carrier card to release the interrupt upon the + acknowledgment. If d=0, the interrupt is released by user interrupt + service routine. Default is 0. MEMOFFS=d this parameter defines the A32 memory access base address. "d" is a decimal number that represents the offset (the upper WORD) of the A32 base address. It is needed when any of the two statements below is true: @@ -150,15 +150,15 @@ the VME slot number in the calculation just makes better logical sense and fits natual of human being's thinking. Some examples are shown below. Let's assume IPMEM=1 (the default setting), this gives 4MB memory space for a 8002 -carrier so starting address line is A22. The remaining must be 0. - +carrier so starting address line is A22. The remaining must be 0. + MEMOFFS BIT 15 14 13 12 11 10 09 08 07 06 05 04 03 02 01 00 A31 A30 A29 A28 A27 A26 A25 A24 A23 A22 0 0 0 0 0 0 For a carrier in VME slot 2, we can define its A32 base address as 0x00400000, plus VME_A32_MSTR_LOCAL. For VME slot 3, it could be 0x00800000 plus VME_A32_MSTR_LOCAL -and for VME slot 4, it could be 0x00C00000 plus VME_A32_MSTR_LOCAL and so forth. - +and for VME slot 4, it could be 0x00C00000 plus VME_A32_MSTR_LOCAL and so forth. + Assuming VME_A32_MSTR_LOCAL is 0x20000000, then for VME slot 4, the calculated base address should be 0x00C00000 + 0x20000000 = 0x20C000000. Hence the MEMOFFS = 8384 (decimal, i.e. 0x20C0). For slot 5, the derived base address could be 0x01000000 + @@ -180,45 +180,45 @@ base address should be 0x01800000 + 0x20000000 = 0x218000000. Hence the MEMOFFS (decimal, i.e. 0x2180). For slot 5, the derived base address could be 0x02000000 + 0x20000000 = 0x22000000. Hence the MEMOFFS = 8704 (decimal, i.e. 0x2200) and so forth. - - -- int Hy8002CarrierInfo(int carrier); - -where 'carrier' is the registered carrier number in the system. If it is specified, -this function prints out the specified carrier hardware information. If carrier = 0xFFFF, -then all carriers' hardware information will be printed out. - - + + +- int Hy8002CarrierInfo(int carrier); + +where 'carrier' is the registered carrier number in the system. If it is specified, +this function prints out the specified carrier hardware information. If carrier = 0xFFFF, +then all carriers' hardware information will be printed out. + + 2). Configuration Examples -------------------------- - -ipacAddHy8002("3,2") - -This indicates that the carrier is in slot 3 and the interrupt level is set to 2. -IP memory uses default 1MB. Clock uses default 8MHz. RORA as default. use geographical -addressing etc. - -ipacAddHy8002("5,4,IPMEM=1,IPCLCK=8,ROAK=1,MEMOFFS=256 ") - -Here the slot is 5, interrupt level is 4. IP memory size is 1MB, clock uses 8MHz. -Use ROAK but not using geographic addressing. The memory offset is 256 which means its -base address is 0x01000000 assuming the VME_A32_MSTR_LOCAL is set to 0x00000000. - +------------------------- + +ipacAddHy8002("3,2") + +This indicates that the carrier is in slot 3 and the interrupt level is set to 2. +IP memory uses default 1MB. Clock uses default 8MHz. RORA as default. use geographical +addressing etc. + +ipacAddHy8002("5,4,IPMEM=1,IPCLCK=8,ROAK=1,MEMOFFS=256 ") + +Here the slot is 5, interrupt level is 4. IP memory size is 1MB, clock uses 8MHz. +Use ROAK but not using geographic addressing. The memory offset is 256 which means its +base address is 0x01000000 assuming the VME_A32_MSTR_LOCAL is set to 0x00000000. + 3). Interrupt Commands Supported -------------------------------- - -The interrupt level can be set by the second parameter of the ipacAddHy8002 routine. -Individual IP module can be set to generate interrupt or not. The commands supported -for ipmIrqCmd are illustrated below. - -cmd Value Returned -ipac_irqGetLevel Carrier interrupt level (0 ~ 7) -ipac_irqEnable 0 = OK -ipac_irqDisable 0 = OK -ipac_irqPoll >0 if the interrupt line is active, else 0 -(other commands) S_IPAC_notImplemented - - +------------------------------- + +The interrupt level can be set by the second parameter of the ipacAddHy8002 routine. +Individual IP module can be set to generate interrupt or not. The commands supported +for ipmIrqCmd are illustrated below. + +cmd Value Returned +ipac_irqGetLevel Carrier interrupt level (0 ~ 7) +ipac_irqEnable 0 = OK +ipac_irqDisable 0 = OK +ipac_irqPoll >0 if the interrupt line is active, else 0 +(other commands) S_IPAC_notImplemented + + Hy8601 IP Asyn Driver Usage ============================ @@ -507,4 +507,4 @@ The following example is for an IOC that uses RTEMS R4.9.4, MVME5500 processor b - + diff --git a/motorApp/MotorSrc/asynMotorAxis.h b/motorApp/MotorSrc/asynMotorAxis.h index 2904b7d8a..96bfda3f9 100644 --- a/motorApp/MotorSrc/asynMotorAxis.h +++ b/motorApp/MotorSrc/asynMotorAxis.h @@ -1,78 +1,78 @@ -/* asynMotorAxis.h - * - * Mark Rivers - * - * This file defines the base class for an asynMotoAxis. It is the class - * from which real motor axes are derived. - */ -#ifndef asynMotorAxis_H -#define asynMotorAxis_H - -#include -#include - -#ifdef __cplusplus -#include - -#include "asynMotorController.h" - -/** Class from which motor axis objects are derived. */ -class epicsShareClass asynMotorAxis { - - public: - /* This is the constructor for the class. */ - asynMotorAxis(class asynMotorController *pController, int axisNumber); - virtual ~asynMotorAxis(); - - virtual asynStatus setIntegerParam(int index, int value); - virtual asynStatus setDoubleParam(int index, double value); - virtual asynStatus setStringParam(int index, const char *value); - virtual void report(FILE *fp, int details); - virtual asynStatus callParamCallbacks(); - - virtual asynStatus move(double position, int relative, double minVelocity, double maxVelocity, double acceleration); - virtual asynStatus moveVelocity(double minVelocity, double maxVelocity, double acceleration); - virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards); - virtual asynStatus stop(double acceleration); - virtual asynStatus poll(bool *moving); - virtual asynStatus setPosition(double position); - virtual asynStatus setEncoderPosition(double position); - virtual asynStatus setHighLimit(double highLimit); - virtual asynStatus setLowLimit(double lowLimit); - virtual asynStatus setPGain(double pGain); - virtual asynStatus setIGain(double iGain); - virtual asynStatus setDGain(double dGain); - virtual asynStatus setClosedLoop(bool closedLoop); - virtual asynStatus setEncoderRatio(double ratio); - virtual asynStatus doMoveToHome(); - - virtual asynStatus initializeProfile(size_t maxPoints); - virtual asynStatus defineProfile(double *positions, size_t numPoints); - virtual asynStatus buildProfile(); - virtual asynStatus executeProfile(); - virtual asynStatus abortProfile(); - virtual asynStatus readbackProfile(); - - void setReferencingModeMove(int distance); - int getReferencingModeMove(); - - protected: - class asynMotorController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. - * Abbreviated because it is used very frequently */ - int axisNo_; /**< Index number of this axis (0 - pC_->numAxes_-1) */ - asynUser *pasynUser_; /**< asynUser connected to this axis for asynTrace debugging */ - double *profilePositions_; /**< Array of target positions for profile moves */ - double *profileReadbacks_; /**< Array of readback positions for profile moves */ - double *profileFollowingErrors_; /**< Array of following errors for profile moves */ - int referencingMode_; - - private: - MotorStatus status_; - int statusChanged_; - int referencingModeMove_; - - friend class asynMotorController; -}; - -#endif /* _cplusplus */ -#endif /* asynMotorAxis_H */ +/* asynMotorAxis.h + * + * Mark Rivers + * + * This file defines the base class for an asynMotoAxis. It is the class + * from which real motor axes are derived. + */ +#ifndef asynMotorAxis_H +#define asynMotorAxis_H + +#include +#include + +#ifdef __cplusplus +#include + +#include "asynMotorController.h" + +/** Class from which motor axis objects are derived. */ +class epicsShareClass asynMotorAxis { + + public: + /* This is the constructor for the class. */ + asynMotorAxis(class asynMotorController *pController, int axisNumber); + virtual ~asynMotorAxis(); + + virtual asynStatus setIntegerParam(int index, int value); + virtual asynStatus setDoubleParam(int index, double value); + virtual asynStatus setStringParam(int index, const char *value); + virtual void report(FILE *fp, int details); + virtual asynStatus callParamCallbacks(); + + virtual asynStatus move(double position, int relative, double minVelocity, double maxVelocity, double acceleration); + virtual asynStatus moveVelocity(double minVelocity, double maxVelocity, double acceleration); + virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards); + virtual asynStatus stop(double acceleration); + virtual asynStatus poll(bool *moving); + virtual asynStatus setPosition(double position); + virtual asynStatus setEncoderPosition(double position); + virtual asynStatus setHighLimit(double highLimit); + virtual asynStatus setLowLimit(double lowLimit); + virtual asynStatus setPGain(double pGain); + virtual asynStatus setIGain(double iGain); + virtual asynStatus setDGain(double dGain); + virtual asynStatus setClosedLoop(bool closedLoop); + virtual asynStatus setEncoderRatio(double ratio); + virtual asynStatus doMoveToHome(); + + virtual asynStatus initializeProfile(size_t maxPoints); + virtual asynStatus defineProfile(double *positions, size_t numPoints); + virtual asynStatus buildProfile(); + virtual asynStatus executeProfile(); + virtual asynStatus abortProfile(); + virtual asynStatus readbackProfile(); + + void setReferencingModeMove(int distance); + int getReferencingModeMove(); + + protected: + class asynMotorController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ + int axisNo_; /**< Index number of this axis (0 - pC_->numAxes_-1) */ + asynUser *pasynUser_; /**< asynUser connected to this axis for asynTrace debugging */ + double *profilePositions_; /**< Array of target positions for profile moves */ + double *profileReadbacks_; /**< Array of readback positions for profile moves */ + double *profileFollowingErrors_; /**< Array of following errors for profile moves */ + int referencingMode_; + + private: + MotorStatus status_; + int statusChanged_; + int referencingModeMove_; + + friend class asynMotorController; +}; + +#endif /* _cplusplus */ +#endif /* asynMotorAxis_H */ diff --git a/motorApp/MotorSrc/asynMotorController.h b/motorApp/MotorSrc/asynMotorController.h index e1ff98682..2edad670f 100644 --- a/motorApp/MotorSrc/asynMotorController.h +++ b/motorApp/MotorSrc/asynMotorController.h @@ -1,301 +1,301 @@ -/* asynMotorController.h - * - * Mark Rivers - * - * This file defines the base class for an asynMotorController. It is the class - * from which real motor controllers are derived. It derives from asynPortDriver. - */ -#ifndef asynMotorController_H -#define asynMotorController_H - -#include -#include - -#define MAX_CONTROLLER_STRING_SIZE 256 -#define DEFAULT_CONTROLLER_TIMEOUT 2.0 - -/** Strings defining parameters for the driver. - * These are the values passed to drvUserCreate. - * The driver will place in pasynUser->reason an integer to be used when the - * standard asyn interface methods are called. */ -#define motorMoveRelString "MOTOR_MOVE_REL" -#define motorMoveAbsString "MOTOR_MOVE_ABS" -#define motorMoveVelString "MOTOR_MOVE_VEL" -#define motorHomeString "MOTOR_HOME" -#define motorStopString "MOTOR_STOP_AXIS" -#define motorVelocityString "MOTOR_VELOCITY" -#define motorVelBaseString "MOTOR_VEL_BASE" -#define motorAccelString "MOTOR_ACCEL" -#define motorPositionString "MOTOR_POSITION" -#define motorEncoderPositionString "MOTOR_ENCODER_POSITION" -#define motorDeferMovesString "MOTOR_DEFER_MOVES" -#define motorMoveToHomeString "MOTOR_MOVE_HOME" -#define motorResolutionString "MOTOR_RESOLUTION" -#define motorEncoderRatioString "MOTOR_ENCODER_RATIO" -#define motorPGainString "MOTOR_PGAIN" -#define motorIGainString "MOTOR_IGAIN" -#define motorDGainString "MOTOR_DGAIN" -#define motorHighLimitString "MOTOR_HIGH_LIMIT" -#define motorLowLimitString "MOTOR_LOW_LIMIT" -#define motorClosedLoopString "MOTOR_CLOSED_LOOP" -#define motorStatusString "MOTOR_STATUS" -#define motorUpdateStatusString "MOTOR_UPDATE_STATUS" -#define motorStatusDirectionString "MOTOR_STATUS_DIRECTION" -#define motorStatusDoneString "MOTOR_STATUS_DONE" -#define motorStatusHighLimitString "MOTOR_STATUS_HIGH_LIMIT" -#define motorStatusAtHomeString "MOTOR_STATUS_AT_HOME" -#define motorStatusSlipString "MOTOR_STATUS_SLIP" -#define motorStatusPowerOnString "MOTOR_STATUS_POWERED" -#define motorStatusFollowingErrorString "MOTOR_STATUS_FOLLOWING_ERROR" -#define motorStatusHomeString "MOTOR_STATUS_HOME" -#define motorStatusHasEncoderString "MOTOR_STATUS_HAS_ENCODER" -#define motorStatusProblemString "MOTOR_STATUS_PROBLEM" -#define motorStatusMovingString "MOTOR_STATUS_MOVING" -#define motorStatusGainSupportString "MOTOR_STATUS_GAIN_SUPPORT" -#define motorStatusCommsErrorString "MOTOR_STATUS_COMMS_ERROR" -#define motorStatusLowLimitString "MOTOR_STATUS_LOW_LIMIT" -#define motorStatusHomedString "MOTOR_STATUS_HOMED" - -/* These are the per-controller parameters for profile moves (coordinated motion) */ -#define profileNumAxesString "PROFILE_NUM_AXES" -#define profileNumPointsString "PROFILE_NUM_POINTS" -#define profileCurrentPointString "PROFILE_CURRENT_POINT" -#define profileNumPulsesString "PROFILE_NUM_PULSES" -#define profileStartPulsesString "PROFILE_START_PULSES" -#define profileEndPulsesString "PROFILE_END_PULSES" -#define profileActualPulsesString "PROFILE_ACTUAL_PULSES" -#define profileNumReadbacksString "PROFILE_NUM_READBACKS" -#define profileTimeModeString "PROFILE_TIME_MODE" -#define profileFixedTimeString "PROFILE_FIXED_TIME" -#define profileTimeArrayString "PROFILE_TIME_ARRAY" -#define profileAccelerationString "PROFILE_ACCELERATION" -#define profileMoveModeString "PROFILE_MOVE_MODE" -#define profileBuildString "PROFILE_BUILD" -#define profileBuildStateString "PROFILE_BUILD_STATE" -#define profileBuildStatusString "PROFILE_BUILD_STATUS" -#define profileBuildMessageString "PROFILE_BUILD_MESSAGE" -#define profileExecuteString "PROFILE_EXECUTE" -#define profileExecuteStateString "PROFILE_EXECUTE_STATE" -#define profileExecuteStatusString "PROFILE_EXECUTE_STATUS" -#define profileExecuteMessageString "PROFILE_EXECUTE_MESSAGE" -#define profileAbortString "PROFILE_ABORT" -#define profileReadbackString "PROFILE_READBACK" -#define profileReadbackStateString "PROFILE_READBACK_STATE" -#define profileReadbackStatusString "PROFILE_READBACK_STATUS" -#define profileReadbackMessageString "PROFILE_READBACK_MESSAGE" - -/* These are the per-axis parameters for profile moves */ -#define profileUseAxisString "PROFILE_USE_AXIS" -#define profilePositionsString "PROFILE_POSITIONS" -#define profileReadbacksString "PROFILE_READBACKS" -#define profileFollowingErrorsString "PROFILE_FOLLOWING_ERRORS" -#define profileMotorResolutionString "PROFILE_MOTOR_RESOLUTION" -#define profileMotorDirectionString "PROFILE_MOTOR_DIRECTION" -#define profileMotorOffsetString "PROFILE_MOTOR_OFFSET" - -/** The structure that is passed back to devMotorAsyn when the status changes. */ -typedef struct MotorStatus { - double position; /**< Commanded motor position */ - double encoderPosition; /**< Actual encoder position */ - double velocity; /**< Actual velocity */ - epicsUInt32 status; /**< Word containing status bits (motion done, limits, etc.) */ -} MotorStatus; - -enum ProfileTimeMode{ - PROFILE_TIME_MODE_FIXED, - PROFILE_TIME_MODE_ARRAY -}; - -enum ProfileMoveMode{ - PROFILE_MOVE_MODE_ABSOLUTE, - PROFILE_MOVE_MODE_RELATIVE -}; - -/* State codes for Build, Read and Execute. Careful, these must match the - * corresponding MBBI records, but there is no way to check this */ -enum ProfileBuildState{ - PROFILE_BUILD_DONE, - PROFILE_BUILD_BUSY -}; - -enum ProfileExecuteState{ - PROFILE_EXECUTE_DONE, - PROFILE_EXECUTE_MOVE_START, - PROFILE_EXECUTE_EXECUTING, - PROFILE_EXECUTE_FLYBACK -}; - -enum ProfileReadbackState{ - PROFILE_READBACK_DONE, - PROFILE_READBACK_BUSY -}; - - -/* Status codes for Build, Execute and Read */ -enum ProfileStatus { - PROFILE_STATUS_UNDEFINED, - PROFILE_STATUS_SUCCESS, - PROFILE_STATUS_FAILURE, - PROFILE_STATUS_ABORT, - PROFILE_STATUS_TIMEOUT -}; - -#ifdef __cplusplus -#include - -class asynMotorAxis; - -class epicsShareClass asynMotorController : public asynPortDriver { - - public: - /* This is the constructor for the class. */ - asynMotorController(const char *portName, int numAxes, int numParams, - int interfaceMask, int interruptMask, - int asynFlags, int autoConnect, int priority, int stackSize); - - virtual ~asynMotorController(); - - /* These are the methods that we override from asynPortDriver */ - virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); - virtual asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); - virtual asynStatus writeFloat64Array(asynUser *pasynUser, epicsFloat64 *value, size_t nElements); - virtual asynStatus readFloat64Array(asynUser *pasynUser, epicsFloat64 *value, size_t nElements, size_t *nRead); - virtual asynStatus readGenericPointer(asynUser *pasynUser, void *pointer); - virtual void report(FILE *fp, int details); - - /* These are the methods that are new to this class */ - virtual asynMotorAxis* getAxis(asynUser *pasynUser); - virtual asynMotorAxis* getAxis(int axisNo); - virtual asynStatus startPoller(double movingPollPeriod, double idlePollPeriod, int forcedFastPolls); - virtual asynStatus wakeupPoller(); - virtual asynStatus poll(); - virtual asynStatus setDeferredMoves(bool defer); - void asynMotorPoller(); // This should be private but is called from C function - - /* Functions to deal with moveToHome.*/ - virtual asynStatus startMoveToHomeThread(); - void asynMotorMoveToHome(); - - /* These are the functions for profile moves */ - virtual asynStatus initializeProfile(size_t maxPoints); - virtual asynStatus buildProfile(); - virtual asynStatus executeProfile(); - virtual asynStatus abortProfile(); - virtual asynStatus readbackProfile(); - - virtual asynStatus setMovingPollPeriod(double movingPollPeriod); - virtual asynStatus setIdlePollPeriod(double idlePollPeriod); - - int shuttingDown_; /**< Flag indicating that IOC is shutting down. Stops poller */ - - protected: - /** These are the index numbers for the parameters in the parameter library. - * They are the values of pasynUser->reason in calls from device support */ - // These are the motor commands - #define FIRST_MOTOR_PARAM motorMoveRel_ - int motorMoveRel_; - int motorMoveAbs_; - int motorMoveVel_; - int motorHome_; - int motorStop_; - int motorVelocity_; - int motorVelBase_; - int motorAccel_; - int motorPosition_; - int motorEncoderPosition_; - int motorDeferMoves_; - int motorMoveToHome_; - int motorResolution_; - int motorEncoderRatio_; - int motorPGain_; - int motorIGain_; - int motorDGain_; - int motorHighLimit_; - int motorLowLimit_; - int motorClosedLoop_; - int motorStatus_; - int motorUpdateStatus_; - - // These are the status bits - int motorStatusDirection_; - int motorStatusDone_; - int motorStatusHighLimit_; - int motorStatusAtHome_; - int motorStatusSlip_; - int motorStatusPowerOn_; - int motorStatusFollowingError_; - int motorStatusHome_; - int motorStatusHasEncoder_; - int motorStatusProblem_; - int motorStatusMoving_; - int motorStatusGainSupport_; - int motorStatusCommsError_; - int motorStatusLowLimit_; - int motorStatusHomed_; - - // These are the per-controller parameters for profile moves - int profileNumAxes_; - int profileNumPoints_; - int profileCurrentPoint_; - int profileNumPulses_; - int profileStartPulses_; - int profileEndPulses_; - int profileActualPulses_; - int profileNumReadbacks_; - int profileTimeMode_; - int profileFixedTime_; - int profileTimeArray_; - int profileAcceleration_; - int profileMoveMode_; - int profileBuild_; - int profileBuildState_; - int profileBuildStatus_; - int profileBuildMessage_; - int profileExecute_; - int profileExecuteState_; - int profileExecuteStatus_; - int profileExecuteMessage_; - int profileAbort_; - int profileReadback_; - int profileReadbackState_; - int profileReadbackStatus_; - int profileReadbackMessage_; - - // These are the per-axis parameters for profile moves - int profileUseAxis_; - int profilePositions_; - int profileReadbacks_; - int profileFollowingErrors_; - int profileMotorResolution_; - int profileMotorDirection_; - int profileMotorOffset_; - #define LAST_MOTOR_PARAM profileMotorOffset_ - - int numAxes_; /**< Number of axes this controller supports */ - asynMotorAxis **pAxes_; /**< Array of pointers to axis objects */ - epicsEventId pollEventId_; /**< Event ID to wake up poller */ - epicsEventId moveToHomeId_; /**< Event ID to wake up move to home thread */ - double idlePollPeriod_; /**< The time between polls when no axes are moving */ - double movingPollPeriod_; /**< The time between polls when any axis is moving */ - int forcedFastPolls_; /**< The number of forced fast polls when the poller wakes up */ - - size_t maxProfilePoints_; /**< Maximum number of profile points */ - double *profileTimes_; /**< Array of times per profile point */ - - int moveToHomeAxis_; - - /* These are convenience functions for controllers that use asynOctet interfaces to the hardware */ - asynStatus writeController(); - asynStatus writeController(const char *output, double timeout); - asynStatus writeReadController(); - asynStatus writeReadController(const char *output, char *response, size_t maxResponseLen, size_t *responseLen, double timeout); - asynUser *pasynUserController_; - char outString_[MAX_CONTROLLER_STRING_SIZE]; - char inString_[MAX_CONTROLLER_STRING_SIZE]; - - friend class asynMotorAxis; -}; -#define NUM_MOTOR_DRIVER_PARAMS (&LAST_MOTOR_PARAM - &FIRST_MOTOR_PARAM + 1) - -#endif /* _cplusplus */ -#endif /* asynMotorController_H */ +/* asynMotorController.h + * + * Mark Rivers + * + * This file defines the base class for an asynMotorController. It is the class + * from which real motor controllers are derived. It derives from asynPortDriver. + */ +#ifndef asynMotorController_H +#define asynMotorController_H + +#include +#include + +#define MAX_CONTROLLER_STRING_SIZE 256 +#define DEFAULT_CONTROLLER_TIMEOUT 2.0 + +/** Strings defining parameters for the driver. + * These are the values passed to drvUserCreate. + * The driver will place in pasynUser->reason an integer to be used when the + * standard asyn interface methods are called. */ +#define motorMoveRelString "MOTOR_MOVE_REL" +#define motorMoveAbsString "MOTOR_MOVE_ABS" +#define motorMoveVelString "MOTOR_MOVE_VEL" +#define motorHomeString "MOTOR_HOME" +#define motorStopString "MOTOR_STOP_AXIS" +#define motorVelocityString "MOTOR_VELOCITY" +#define motorVelBaseString "MOTOR_VEL_BASE" +#define motorAccelString "MOTOR_ACCEL" +#define motorPositionString "MOTOR_POSITION" +#define motorEncoderPositionString "MOTOR_ENCODER_POSITION" +#define motorDeferMovesString "MOTOR_DEFER_MOVES" +#define motorMoveToHomeString "MOTOR_MOVE_HOME" +#define motorResolutionString "MOTOR_RESOLUTION" +#define motorEncoderRatioString "MOTOR_ENCODER_RATIO" +#define motorPGainString "MOTOR_PGAIN" +#define motorIGainString "MOTOR_IGAIN" +#define motorDGainString "MOTOR_DGAIN" +#define motorHighLimitString "MOTOR_HIGH_LIMIT" +#define motorLowLimitString "MOTOR_LOW_LIMIT" +#define motorClosedLoopString "MOTOR_CLOSED_LOOP" +#define motorStatusString "MOTOR_STATUS" +#define motorUpdateStatusString "MOTOR_UPDATE_STATUS" +#define motorStatusDirectionString "MOTOR_STATUS_DIRECTION" +#define motorStatusDoneString "MOTOR_STATUS_DONE" +#define motorStatusHighLimitString "MOTOR_STATUS_HIGH_LIMIT" +#define motorStatusAtHomeString "MOTOR_STATUS_AT_HOME" +#define motorStatusSlipString "MOTOR_STATUS_SLIP" +#define motorStatusPowerOnString "MOTOR_STATUS_POWERED" +#define motorStatusFollowingErrorString "MOTOR_STATUS_FOLLOWING_ERROR" +#define motorStatusHomeString "MOTOR_STATUS_HOME" +#define motorStatusHasEncoderString "MOTOR_STATUS_HAS_ENCODER" +#define motorStatusProblemString "MOTOR_STATUS_PROBLEM" +#define motorStatusMovingString "MOTOR_STATUS_MOVING" +#define motorStatusGainSupportString "MOTOR_STATUS_GAIN_SUPPORT" +#define motorStatusCommsErrorString "MOTOR_STATUS_COMMS_ERROR" +#define motorStatusLowLimitString "MOTOR_STATUS_LOW_LIMIT" +#define motorStatusHomedString "MOTOR_STATUS_HOMED" + +/* These are the per-controller parameters for profile moves (coordinated motion) */ +#define profileNumAxesString "PROFILE_NUM_AXES" +#define profileNumPointsString "PROFILE_NUM_POINTS" +#define profileCurrentPointString "PROFILE_CURRENT_POINT" +#define profileNumPulsesString "PROFILE_NUM_PULSES" +#define profileStartPulsesString "PROFILE_START_PULSES" +#define profileEndPulsesString "PROFILE_END_PULSES" +#define profileActualPulsesString "PROFILE_ACTUAL_PULSES" +#define profileNumReadbacksString "PROFILE_NUM_READBACKS" +#define profileTimeModeString "PROFILE_TIME_MODE" +#define profileFixedTimeString "PROFILE_FIXED_TIME" +#define profileTimeArrayString "PROFILE_TIME_ARRAY" +#define profileAccelerationString "PROFILE_ACCELERATION" +#define profileMoveModeString "PROFILE_MOVE_MODE" +#define profileBuildString "PROFILE_BUILD" +#define profileBuildStateString "PROFILE_BUILD_STATE" +#define profileBuildStatusString "PROFILE_BUILD_STATUS" +#define profileBuildMessageString "PROFILE_BUILD_MESSAGE" +#define profileExecuteString "PROFILE_EXECUTE" +#define profileExecuteStateString "PROFILE_EXECUTE_STATE" +#define profileExecuteStatusString "PROFILE_EXECUTE_STATUS" +#define profileExecuteMessageString "PROFILE_EXECUTE_MESSAGE" +#define profileAbortString "PROFILE_ABORT" +#define profileReadbackString "PROFILE_READBACK" +#define profileReadbackStateString "PROFILE_READBACK_STATE" +#define profileReadbackStatusString "PROFILE_READBACK_STATUS" +#define profileReadbackMessageString "PROFILE_READBACK_MESSAGE" + +/* These are the per-axis parameters for profile moves */ +#define profileUseAxisString "PROFILE_USE_AXIS" +#define profilePositionsString "PROFILE_POSITIONS" +#define profileReadbacksString "PROFILE_READBACKS" +#define profileFollowingErrorsString "PROFILE_FOLLOWING_ERRORS" +#define profileMotorResolutionString "PROFILE_MOTOR_RESOLUTION" +#define profileMotorDirectionString "PROFILE_MOTOR_DIRECTION" +#define profileMotorOffsetString "PROFILE_MOTOR_OFFSET" + +/** The structure that is passed back to devMotorAsyn when the status changes. */ +typedef struct MotorStatus { + double position; /**< Commanded motor position */ + double encoderPosition; /**< Actual encoder position */ + double velocity; /**< Actual velocity */ + epicsUInt32 status; /**< Word containing status bits (motion done, limits, etc.) */ +} MotorStatus; + +enum ProfileTimeMode{ + PROFILE_TIME_MODE_FIXED, + PROFILE_TIME_MODE_ARRAY +}; + +enum ProfileMoveMode{ + PROFILE_MOVE_MODE_ABSOLUTE, + PROFILE_MOVE_MODE_RELATIVE +}; + +/* State codes for Build, Read and Execute. Careful, these must match the + * corresponding MBBI records, but there is no way to check this */ +enum ProfileBuildState{ + PROFILE_BUILD_DONE, + PROFILE_BUILD_BUSY +}; + +enum ProfileExecuteState{ + PROFILE_EXECUTE_DONE, + PROFILE_EXECUTE_MOVE_START, + PROFILE_EXECUTE_EXECUTING, + PROFILE_EXECUTE_FLYBACK +}; + +enum ProfileReadbackState{ + PROFILE_READBACK_DONE, + PROFILE_READBACK_BUSY +}; + + +/* Status codes for Build, Execute and Read */ +enum ProfileStatus { + PROFILE_STATUS_UNDEFINED, + PROFILE_STATUS_SUCCESS, + PROFILE_STATUS_FAILURE, + PROFILE_STATUS_ABORT, + PROFILE_STATUS_TIMEOUT +}; + +#ifdef __cplusplus +#include + +class asynMotorAxis; + +class epicsShareClass asynMotorController : public asynPortDriver { + + public: + /* This is the constructor for the class. */ + asynMotorController(const char *portName, int numAxes, int numParams, + int interfaceMask, int interruptMask, + int asynFlags, int autoConnect, int priority, int stackSize); + + virtual ~asynMotorController(); + + /* These are the methods that we override from asynPortDriver */ + virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + virtual asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); + virtual asynStatus writeFloat64Array(asynUser *pasynUser, epicsFloat64 *value, size_t nElements); + virtual asynStatus readFloat64Array(asynUser *pasynUser, epicsFloat64 *value, size_t nElements, size_t *nRead); + virtual asynStatus readGenericPointer(asynUser *pasynUser, void *pointer); + virtual void report(FILE *fp, int details); + + /* These are the methods that are new to this class */ + virtual asynMotorAxis* getAxis(asynUser *pasynUser); + virtual asynMotorAxis* getAxis(int axisNo); + virtual asynStatus startPoller(double movingPollPeriod, double idlePollPeriod, int forcedFastPolls); + virtual asynStatus wakeupPoller(); + virtual asynStatus poll(); + virtual asynStatus setDeferredMoves(bool defer); + void asynMotorPoller(); // This should be private but is called from C function + + /* Functions to deal with moveToHome.*/ + virtual asynStatus startMoveToHomeThread(); + void asynMotorMoveToHome(); + + /* These are the functions for profile moves */ + virtual asynStatus initializeProfile(size_t maxPoints); + virtual asynStatus buildProfile(); + virtual asynStatus executeProfile(); + virtual asynStatus abortProfile(); + virtual asynStatus readbackProfile(); + + virtual asynStatus setMovingPollPeriod(double movingPollPeriod); + virtual asynStatus setIdlePollPeriod(double idlePollPeriod); + + int shuttingDown_; /**< Flag indicating that IOC is shutting down. Stops poller */ + + protected: + /** These are the index numbers for the parameters in the parameter library. + * They are the values of pasynUser->reason in calls from device support */ + // These are the motor commands + #define FIRST_MOTOR_PARAM motorMoveRel_ + int motorMoveRel_; + int motorMoveAbs_; + int motorMoveVel_; + int motorHome_; + int motorStop_; + int motorVelocity_; + int motorVelBase_; + int motorAccel_; + int motorPosition_; + int motorEncoderPosition_; + int motorDeferMoves_; + int motorMoveToHome_; + int motorResolution_; + int motorEncoderRatio_; + int motorPGain_; + int motorIGain_; + int motorDGain_; + int motorHighLimit_; + int motorLowLimit_; + int motorClosedLoop_; + int motorStatus_; + int motorUpdateStatus_; + + // These are the status bits + int motorStatusDirection_; + int motorStatusDone_; + int motorStatusHighLimit_; + int motorStatusAtHome_; + int motorStatusSlip_; + int motorStatusPowerOn_; + int motorStatusFollowingError_; + int motorStatusHome_; + int motorStatusHasEncoder_; + int motorStatusProblem_; + int motorStatusMoving_; + int motorStatusGainSupport_; + int motorStatusCommsError_; + int motorStatusLowLimit_; + int motorStatusHomed_; + + // These are the per-controller parameters for profile moves + int profileNumAxes_; + int profileNumPoints_; + int profileCurrentPoint_; + int profileNumPulses_; + int profileStartPulses_; + int profileEndPulses_; + int profileActualPulses_; + int profileNumReadbacks_; + int profileTimeMode_; + int profileFixedTime_; + int profileTimeArray_; + int profileAcceleration_; + int profileMoveMode_; + int profileBuild_; + int profileBuildState_; + int profileBuildStatus_; + int profileBuildMessage_; + int profileExecute_; + int profileExecuteState_; + int profileExecuteStatus_; + int profileExecuteMessage_; + int profileAbort_; + int profileReadback_; + int profileReadbackState_; + int profileReadbackStatus_; + int profileReadbackMessage_; + + // These are the per-axis parameters for profile moves + int profileUseAxis_; + int profilePositions_; + int profileReadbacks_; + int profileFollowingErrors_; + int profileMotorResolution_; + int profileMotorDirection_; + int profileMotorOffset_; + #define LAST_MOTOR_PARAM profileMotorOffset_ + + int numAxes_; /**< Number of axes this controller supports */ + asynMotorAxis **pAxes_; /**< Array of pointers to axis objects */ + epicsEventId pollEventId_; /**< Event ID to wake up poller */ + epicsEventId moveToHomeId_; /**< Event ID to wake up move to home thread */ + double idlePollPeriod_; /**< The time between polls when no axes are moving */ + double movingPollPeriod_; /**< The time between polls when any axis is moving */ + int forcedFastPolls_; /**< The number of forced fast polls when the poller wakes up */ + + size_t maxProfilePoints_; /**< Maximum number of profile points */ + double *profileTimes_; /**< Array of times per profile point */ + + int moveToHomeAxis_; + + /* These are convenience functions for controllers that use asynOctet interfaces to the hardware */ + asynStatus writeController(); + asynStatus writeController(const char *output, double timeout); + asynStatus writeReadController(); + asynStatus writeReadController(const char *output, char *response, size_t maxResponseLen, size_t *responseLen, double timeout); + asynUser *pasynUserController_; + char outString_[MAX_CONTROLLER_STRING_SIZE]; + char inString_[MAX_CONTROLLER_STRING_SIZE]; + + friend class asynMotorAxis; +}; +#define NUM_MOTOR_DRIVER_PARAMS (&LAST_MOTOR_PARAM - &FIRST_MOTOR_PARAM + 1) + +#endif /* _cplusplus */ +#endif /* asynMotorController_H */ diff --git a/motorApp/NewportSrc/XPS_C8_errors.h b/motorApp/NewportSrc/XPS_C8_errors.h index cd7087523..2098187d5 100644 --- a/motorApp/NewportSrc/XPS_C8_errors.h +++ b/motorApp/NewportSrc/XPS_C8_errors.h @@ -1,107 +1,107 @@ -/*/////////////////////////////////////////////////////////////////// - * Created header file XPS_C8_errors.h for XPS function errors - */ - -/* TCL interpretor error */ -#define ERR_TCL_INTERPRETOR_ERROR 1 - -/* No error */ -#define SUCCESS 0 - -/* XPS errors */ -#define ERR_BUSY_SOCKET -1 -#define ERR_TCP_TIMEOUT -2 -#define ERR_STRING_TOO_LONG -3 -#define ERR_UNKNOWN_COMMAND -4 -#define ERR_POSITIONER_ERROR -5 -#define ERR_WRONG_FORMAT -7 -#define ERR_WRONG_OBJECT_TYPE -8 -#define ERR_WRONG_PARAMETERS_NUMBER -9 -#define ERR_WRONG_TYPE -10 -#define ERR_WRONG_TYPE_BIT_WORD -11 -#define ERR_WRONG_TYPE_BOOL -12 -#define ERR_WRONG_TYPE_CHAR -13 -#define ERR_WRONG_TYPE_DOUBLE -14 -#define ERR_WRONG_TYPE_INT -15 -#define ERR_WRONG_TYPE_UNSIGNEDINT -16 -#define ERR_PARAMETER_OUT_OF_RANGE -17 -#define ERR_POSITIONER_NAME -18 -#define ERR_GROUP_NAME -19 -#define ERR_FATAL_INIT -20 -#define ERR_IN_INITIALIZATION -21 -#define ERR_NOT_ALLOWED_ACTION -22 -#define ERR_POSITION_COMPARE_NOT_SET -23 -#define ERR_UNCOMPATIBLE -24 -#define ERR_FOLLOWING_ERROR -25 -#define ERR_EMERGENCY_SIGNAL -26 -#define ERR_GROUP_ABORT_MOTION -27 -#define ERR_GROUP_HOME_SEARCH_TIMEOUT -28 -#define ERR_MNEMOTYPEGATHERING -29 -#define ERR_GATHERING_NOT_STARTED -30 -#define ERR_HOME_OUT_RANGE -31 -#define ERR_GATHERING_NOT_CONFIGURED -32 -#define ERR_GROUP_MOTION_DONE_TIMEOUT -33 -#define ERR_TRAVEL_LIMITS -35 -#define ERR_UNKNOWN_TCL_FILE -36 -#define ERR_TCL_SCRIPT_KILL -38 -#define ERR_TCL_INTERPRETOR -37 -#define ERR_MNEMO_ACTION -39 -#define ERR_MNEMO_EVENT -40 -#define ERR_SLAVE_CONFIGURATION -41 -#define ERR_JOG_OUT_OF_RANGE -42 -#define ERR_GATHERING_RUNNING -43 -#define ERR_SLAVE -44 -#define ERR_END_OF_RUN -45 -#define ERR_NOT_ALLOWED_BACKLASH -46 -#define ERR_WRONG_TCL_TASKNAME -47 -#define ERR_BASE_VELOCITY -48 -#define ERR_GROUP_HOME_SEARCH_ZM_ERROR -49 -#define ERR_MOTOR_INITIALIZATION_ERROR -50 -#define ERR_SPIN_OUT_OF_RANGE -51 -#define ERR_WRITE_FILE -60 -#define ERR_READ_FILE -61 -#define ERR_TRAJ_ELEM_TYPE -62 -#define ERR_TRAJ_ELEM_RADIUS -63 -#define ERR_TRAJ_ELEM_SWEEP -64 -#define ERR_TRAJ_ELEM_LINE -65 -#define ERR_TRAJ_EMPTY -66 -#define ERR_TRAJ_VEL_LIMIT -68 -#define ERR_TRAJ_ACC_LIMIT -69 -#define ERR_TRAJ_FINAL_VELOCITY -70 -#define ERR_MSG_QUEUE -71 -#define ERR_TRAJ_INITIALIZATION -72 -#define ERR_END_OF_FILE -73 -#define ERR_READ_FILE_PARAMETER_KEY -74 -#define ERR_TRAJ_TIME -75 -#define ERR_EVENTS_NOT_CONFIGURED -80 -#define ERR_ACTIONS_NOT_CONFIGURED -81 -#define ERR_EVENT_BUFFER_FULL -82 -#define ERR_EVENT_ID_UNDEFINED -83 -#define ERR_HOME_SEARCH_GANTRY_TOLERANCE_ERROR -85 -#define ERR_FOCUS_RESERVED_SOCKET -90 -#define ERR_FOCUS_BUSY_EVENT_SCHEDULER -91 -#define ERR_OPTIONAL_EXTERNAL_MODULE_FILE -94 -#define ERR_OPTIONAL_EXTERNAL_MODULE_EXECUTE -95 -#define ERR_OPTIONAL_EXTERNAL_MODULE_KILL -96 -#define ERR_OPTIONAL_EXTERNAL_MODULE_LOAD -97 -#define ERR_OPTIONAL_EXTERNAL_MODULE_UNLOAD -98 -#define ERR_FATAL_EXTERNAL_MODULE_LOAD -99 -#define ERR_INTERNAL_ERROR -100 -#define ERR_RELAY_FEEDBACK_TEST_NO_OSCILLATION -101 -#define ERR_RELAY_FEEDBACK_TEST_SIGNAL_NOISY -102 -#define ERR_SIGNAL_POINTS_NOT_ENOUGH -103 -#define ERR_PID_TUNING_INITIALIZATION -104 -#define ERR_SCALING_CALIBRATION -105 -#define ERR_WRONG_USERNAME_OR_PASSWORD -106 -#define ERR_NEED_ADMINISTRATOR_RIGHTS -107 -#define ERR_SOCKET_CLOSED_BY_ADMIN -108 -#define ERR_NEED_TO_BE_HOMED_AT_LEAST_ONCE -109 -#define ERR_NOT_ALLOWED_FOR_GANTRY -110 -#define ERR_GATHERING_BUFFER_FULL -111 -#define ERR_EXCITATION_SIGNAL_INITIALIZATION -112 -#define ERR_BOTH_ENDS_OF_RUNS_ACTIVATED -113 -#define ERR_GROUP_CLAMPING_TIMEOUT -114 -#define ERR_HARDWARE_FUNCTION_NOT_SUPPORTED -115 -#define ERR_EXTERNAL_DRIVER_INIT -116 -#define ERR_FUNCTION_ONLY_ALLOWED_IN_DISABLED_STATE -117 -#define ERR_NOT_ALLOWED_DRIVER_NOT_INITIALIZED -118 +/*/////////////////////////////////////////////////////////////////// + * Created header file XPS_C8_errors.h for XPS function errors + */ + +/* TCL interpretor error */ +#define ERR_TCL_INTERPRETOR_ERROR 1 + +/* No error */ +#define SUCCESS 0 + +/* XPS errors */ +#define ERR_BUSY_SOCKET -1 +#define ERR_TCP_TIMEOUT -2 +#define ERR_STRING_TOO_LONG -3 +#define ERR_UNKNOWN_COMMAND -4 +#define ERR_POSITIONER_ERROR -5 +#define ERR_WRONG_FORMAT -7 +#define ERR_WRONG_OBJECT_TYPE -8 +#define ERR_WRONG_PARAMETERS_NUMBER -9 +#define ERR_WRONG_TYPE -10 +#define ERR_WRONG_TYPE_BIT_WORD -11 +#define ERR_WRONG_TYPE_BOOL -12 +#define ERR_WRONG_TYPE_CHAR -13 +#define ERR_WRONG_TYPE_DOUBLE -14 +#define ERR_WRONG_TYPE_INT -15 +#define ERR_WRONG_TYPE_UNSIGNEDINT -16 +#define ERR_PARAMETER_OUT_OF_RANGE -17 +#define ERR_POSITIONER_NAME -18 +#define ERR_GROUP_NAME -19 +#define ERR_FATAL_INIT -20 +#define ERR_IN_INITIALIZATION -21 +#define ERR_NOT_ALLOWED_ACTION -22 +#define ERR_POSITION_COMPARE_NOT_SET -23 +#define ERR_UNCOMPATIBLE -24 +#define ERR_FOLLOWING_ERROR -25 +#define ERR_EMERGENCY_SIGNAL -26 +#define ERR_GROUP_ABORT_MOTION -27 +#define ERR_GROUP_HOME_SEARCH_TIMEOUT -28 +#define ERR_MNEMOTYPEGATHERING -29 +#define ERR_GATHERING_NOT_STARTED -30 +#define ERR_HOME_OUT_RANGE -31 +#define ERR_GATHERING_NOT_CONFIGURED -32 +#define ERR_GROUP_MOTION_DONE_TIMEOUT -33 +#define ERR_TRAVEL_LIMITS -35 +#define ERR_UNKNOWN_TCL_FILE -36 +#define ERR_TCL_SCRIPT_KILL -38 +#define ERR_TCL_INTERPRETOR -37 +#define ERR_MNEMO_ACTION -39 +#define ERR_MNEMO_EVENT -40 +#define ERR_SLAVE_CONFIGURATION -41 +#define ERR_JOG_OUT_OF_RANGE -42 +#define ERR_GATHERING_RUNNING -43 +#define ERR_SLAVE -44 +#define ERR_END_OF_RUN -45 +#define ERR_NOT_ALLOWED_BACKLASH -46 +#define ERR_WRONG_TCL_TASKNAME -47 +#define ERR_BASE_VELOCITY -48 +#define ERR_GROUP_HOME_SEARCH_ZM_ERROR -49 +#define ERR_MOTOR_INITIALIZATION_ERROR -50 +#define ERR_SPIN_OUT_OF_RANGE -51 +#define ERR_WRITE_FILE -60 +#define ERR_READ_FILE -61 +#define ERR_TRAJ_ELEM_TYPE -62 +#define ERR_TRAJ_ELEM_RADIUS -63 +#define ERR_TRAJ_ELEM_SWEEP -64 +#define ERR_TRAJ_ELEM_LINE -65 +#define ERR_TRAJ_EMPTY -66 +#define ERR_TRAJ_VEL_LIMIT -68 +#define ERR_TRAJ_ACC_LIMIT -69 +#define ERR_TRAJ_FINAL_VELOCITY -70 +#define ERR_MSG_QUEUE -71 +#define ERR_TRAJ_INITIALIZATION -72 +#define ERR_END_OF_FILE -73 +#define ERR_READ_FILE_PARAMETER_KEY -74 +#define ERR_TRAJ_TIME -75 +#define ERR_EVENTS_NOT_CONFIGURED -80 +#define ERR_ACTIONS_NOT_CONFIGURED -81 +#define ERR_EVENT_BUFFER_FULL -82 +#define ERR_EVENT_ID_UNDEFINED -83 +#define ERR_HOME_SEARCH_GANTRY_TOLERANCE_ERROR -85 +#define ERR_FOCUS_RESERVED_SOCKET -90 +#define ERR_FOCUS_BUSY_EVENT_SCHEDULER -91 +#define ERR_OPTIONAL_EXTERNAL_MODULE_FILE -94 +#define ERR_OPTIONAL_EXTERNAL_MODULE_EXECUTE -95 +#define ERR_OPTIONAL_EXTERNAL_MODULE_KILL -96 +#define ERR_OPTIONAL_EXTERNAL_MODULE_LOAD -97 +#define ERR_OPTIONAL_EXTERNAL_MODULE_UNLOAD -98 +#define ERR_FATAL_EXTERNAL_MODULE_LOAD -99 +#define ERR_INTERNAL_ERROR -100 +#define ERR_RELAY_FEEDBACK_TEST_NO_OSCILLATION -101 +#define ERR_RELAY_FEEDBACK_TEST_SIGNAL_NOISY -102 +#define ERR_SIGNAL_POINTS_NOT_ENOUGH -103 +#define ERR_PID_TUNING_INITIALIZATION -104 +#define ERR_SCALING_CALIBRATION -105 +#define ERR_WRONG_USERNAME_OR_PASSWORD -106 +#define ERR_NEED_ADMINISTRATOR_RIGHTS -107 +#define ERR_SOCKET_CLOSED_BY_ADMIN -108 +#define ERR_NEED_TO_BE_HOMED_AT_LEAST_ONCE -109 +#define ERR_NOT_ALLOWED_FOR_GANTRY -110 +#define ERR_GATHERING_BUFFER_FULL -111 +#define ERR_EXCITATION_SIGNAL_INITIALIZATION -112 +#define ERR_BOTH_ENDS_OF_RUNS_ACTIVATED -113 +#define ERR_GROUP_CLAMPING_TIMEOUT -114 +#define ERR_HARDWARE_FUNCTION_NOT_SUPPORTED -115 +#define ERR_EXTERNAL_DRIVER_INIT -116 +#define ERR_FUNCTION_ONLY_ALLOWED_IN_DISABLED_STATE -117 +#define ERR_NOT_ALLOWED_DRIVER_NOT_INITIALIZED -118 diff --git a/motorApp/NewportSrc/hxp_drivers.cpp b/motorApp/NewportSrc/hxp_drivers.cpp index 1d002182a..1728f65be 100644 --- a/motorApp/NewportSrc/hxp_drivers.cpp +++ b/motorApp/NewportSrc/hxp_drivers.cpp @@ -1,6904 +1,6904 @@ -/* - * Created source file hxp_drivers.cpp for API description - */ - - -#include -#include -#include -#include -#include "Socket.h" - -#ifdef _WIN32 - #define DLL _declspec(dllexport) - #include "strtok_r.h" -#else - #define DLL -#endif - -#include "hxp_drivers.h" - - - -#define SIZE_SMALL 1024 -#define SIZE_NOMINAL 1024 -#define SIZE_BIG 2048 -#define SIZE_HUGE 65536 - -#define SIZE_EXECUTE_METHOD 1024 - -#define SIZE_NAME 100 -#ifdef __cplusplus -extern "C" -{ -#else -#typedef int bool; /* C does not know bool, only C++ */ -#endif - - - -#define DLL_VERSION "Library version for HXP Firmware V1.3.x" - -/************************************************************************* -* Replace 'oldChar' by 'newChar' only between the startChar and endChar -*************************************************************************/ -void ReplaceCharacter (char *strSourceInOut, char oldChar, char newChar, char startChar, char endChar) -{ - char *pt; - char *ptNext; - ptNext = strSourceInOut; - do - { - pt = strchr(ptNext, startChar); - if (pt != NULL) - { - *pt++; - while ((pt != NULL) && (*pt != endChar)) - { - if (*pt == oldChar) - *pt = newChar; - pt++; - } - ptNext = pt++; - } - } - while ((pt != NULL) && (ptNext != NULL)); -} - -/************************************************************************* -* Delete space and tabulation characters between 'startChar' and 'endChar' -*************************************************************************/ -void CleanString (char *strSourceInOut, char startChar, char endChar) -{ - int len = 0; - int startIndex = 0; - int endIndex = 0; - int outputIndex = 0; - char outputString[SIZE_NOMINAL]; - len = strlen(strSourceInOut); - do - { - while ((strSourceInOut[startIndex] != startChar) && (startIndex < len) && (outputIndex < SIZE_NOMINAL)) - { - outputString[outputIndex] = strSourceInOut[startIndex]; - outputIndex++; - startIndex++; - } - while ((strSourceInOut[endIndex] != endChar) && (endIndex < len)) - endIndex++; - if ((startIndex != endIndex) && (startIndex < len)) - { - for (int i = startIndex; (i <= endIndex) && (outputIndex < SIZE_NOMINAL); i++) - { - if ((strSourceInOut[i] != ' ') && (strSourceInOut[i] != '\t')) - { - outputString[outputIndex] = strSourceInOut[i]; - outputIndex++; - } - } - endIndex++; - startIndex = endIndex; - } - } - while (startIndex < len); - outputString[outputIndex] = '\0'; - strcpy (strSourceInOut, outputString); -} - -/************************************************************************* -* Delete a specified characters -*************************************************************************/ -void DeleteCharacters (char *strSourceInOut, char *charactersToDelete) -{ - int len = 0; - int nbChar = 0; - int outputIndex = 0; - bool bCopy; - char outputString[SIZE_NOMINAL]; - len = strlen(strSourceInOut); - nbChar = strlen(charactersToDelete); - for (int i = 0; (i <= len) && (outputIndex < SIZE_NOMINAL); i++) - { - bCopy = true; - for (int j = 0; (j < nbChar) && (bCopy == true); j++) - { - if (strSourceInOut[i] == charactersToDelete[j]) - bCopy = false; - } - if (bCopy) - { - outputString[outputIndex] = strSourceInOut[i]; - outputIndex++; - } - } - outputString[outputIndex] = '\0'; - strcpy (strSourceInOut, outputString); -} - -/***********************************************************************/ -int __stdcall HXPTCP_ConnectToServer(char *Ip_Address, int Ip_Port, double TimeOut) -{ - return (ConnectToServer(Ip_Address, Ip_Port, TimeOut)); -} -/***********************************************************************/ -void __stdcall HXPTCP_SetTimeout(int SocketIndex, double Timeout) -{ - SetTCPTimeout(SocketIndex, Timeout); -} -/***********************************************************************/ -void __stdcall HXPTCP_CloseSocket(int SocketIndex) -{ - CloseSocket(SocketIndex); -} -/***********************************************************************/ -char * __stdcall HXPTCP_GetError(int SocketIndex) -{ - return (GetError(SocketIndex)); -} -/***********************************************************************/ -char * __stdcall HXPGetLibraryVersion(void) -{ - return (DLL_VERSION); -} - -/*********************************************************************** - * ControllerMotionKernelTimeLoadGet : Get controller motion kernel time load - * - * - Parameters : - * int SocketIndex - * double *CPUTotalLoadRatio - * double *CPUCorrectorLoadRatio - * double *CPUProfilerLoadRatio - * double *CPUServitudesLoadRatio - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPControllerMotionKernelTimeLoadGet (int SocketIndex, double * CPUTotalLoadRatio, double * CPUCorrectorLoadRatio, double * CPUProfilerLoadRatio, double * CPUServitudesLoadRatio) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "ControllerMotionKernelTimeLoadGet (double *,double *,double *,double *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CPUTotalLoadRatio); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CPUCorrectorLoadRatio); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CPUProfilerLoadRatio); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CPUServitudesLoadRatio); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ElapsedTimeGet : Return elapsed time from controller power on - * - * - Parameters : - * int SocketIndex - * double *ElapsedTime - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPElapsedTimeGet (int SocketIndex, double * ElapsedTime) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "ElapsedTimeGet (double *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", ElapsedTime); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ErrorStringGet : Return the error string corresponding to the error code - * - * - Parameters : - * int SocketIndex - * int ErrorCode - * char *ErrorString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPErrorStringGet (int SocketIndex, int ErrorCode, char * ErrorString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "ErrorStringGet (%d,char *)", ErrorCode); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ErrorString, pt); - ptNext = strchr (ErrorString, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * FirmwareVersionGet : Return firmware version - * - * - Parameters : - * int SocketIndex - * char *Version - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPFirmwareVersionGet (int SocketIndex, char * Version) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "FirmwareVersionGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (Version, pt); - ptNext = strchr (Version, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * TCLScriptExecute : Execute a TCL script from a TCL file - * - * - Parameters : - * int SocketIndex - * char *TCLFileName - * char *TaskName - * char *ParametersList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPTCLScriptExecute (int SocketIndex, char * TCLFileName, char * TaskName, char * ParametersList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "TCLScriptExecute (%s,%s,%s)", TCLFileName, TaskName, ParametersList); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * TCLScriptExecuteAndWait : Execute a TCL script from a TCL file and wait the end of execution to return - * - * - Parameters : - * int SocketIndex - * char *TCLFileName - * char *TaskName - * char *InputParametersList - * char *OutputParametersList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPTCLScriptExecuteAndWait (int SocketIndex, char * TCLFileName, char * TaskName, char * InputParametersList, char * OutputParametersList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "TCLScriptExecuteAndWait (%s,%s,%s,char *)", TCLFileName, TaskName, InputParametersList); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (OutputParametersList, pt); - ptNext = strchr (OutputParametersList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * TCLScriptKill : Kill TCL Task - * - * - Parameters : - * int SocketIndex - * char *TaskName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPTCLScriptKill (int SocketIndex, char * TaskName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "TCLScriptKill (%s)", TaskName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * TimerGet : Get a timer - * - * - Parameters : - * int SocketIndex - * char *TimerName - * int *FrequencyTicks - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPTimerGet (int SocketIndex, char * TimerName, int * FrequencyTicks) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "TimerGet (%s,int *)", TimerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", FrequencyTicks); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * TimerSet : Set a timer - * - * - Parameters : - * int SocketIndex - * char *TimerName - * int FrequencyTicks - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPTimerSet (int SocketIndex, char * TimerName, int FrequencyTicks) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "TimerSet (%s,%d)", TimerName, FrequencyTicks); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * Reboot : Reboot the controller - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPReboot (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "Reboot ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * Login : Log in - * - * - Parameters : - * int SocketIndex - * char *Name - * char *Password - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPLogin (int SocketIndex, char * Name, char * Password) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "Login (%s,%s)", Name, Password); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * CloseAllOtherSockets : Close all socket beside the one used to send this command - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPCloseAllOtherSockets (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "CloseAllOtherSockets ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventAdd : ** OBSOLETE ** Add an event - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * char *EventName - * char *EventParameter - * char *ActionName - * char *ActionParameter1 - * char *ActionParameter2 - * char *ActionParameter3 - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventAdd (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter, char * ActionName, char * ActionParameter1, char * ActionParameter2, char * ActionParameter3) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventAdd (%s,%s,%s,%s,%s,%s,%s)", PositionerName, EventName, EventParameter, ActionName, ActionParameter1, ActionParameter2, ActionParameter3); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventGet : ** OBSOLETE ** Read events and actions list - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * char *EventsAndActionsList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventGet (int SocketIndex, char * PositionerName, char * EventsAndActionsList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventGet (%s,char *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (EventsAndActionsList, pt); - ptNext = strchr (EventsAndActionsList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventRemove : ** OBSOLETE ** Delete an event - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * char *EventName - * char *EventParameter - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventRemove (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventRemove (%s,%s,%s)", PositionerName, EventName, EventParameter); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventWait : ** OBSOLETE ** Wait an event - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * char *EventName - * char *EventParameter - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventWait (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventWait (%s,%s,%s)", PositionerName, EventName, EventParameter); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedConfigurationTriggerSet : Configure one or several events - * - * - Parameters : - * int SocketIndex - * int nbElement - * char *ExtendedEventName - * char *EventParameter1 - * char *EventParameter2 - * char *EventParameter3 - * char *EventParameter4 - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedConfigurationTriggerSet (int SocketIndex, int NbElements, char * ExtendedEventNameList, char * EventParameter1List, char * EventParameter2List, char * EventParameter3List, char * EventParameter4List) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Split list */ - char *token; - char seps[] = " \t;"; - int indice; - char list [SIZE_NOMINAL]; - char *list_r; - char subString[] = "{}"; - - char (*stringArray0)[SIZE_NAME]; - stringArray0 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, ExtendedEventNameList, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray0[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray0[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - char (*stringArray1)[SIZE_NAME]; - stringArray1 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, EventParameter1List, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray1[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray1[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray1[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray1[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - char (*stringArray2)[SIZE_NAME]; - stringArray2 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, EventParameter2List, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray2[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray2[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray2[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray2[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - char (*stringArray3)[SIZE_NAME]; - stringArray3 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, EventParameter3List, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray3[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray3[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray3[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray3[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - char (*stringArray4)[SIZE_NAME]; - stringArray4 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, EventParameter4List, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray4[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray4[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray4[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray4[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedConfigurationTriggerSet ("); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%s,%s,%s,%s,%s", stringArray0[i], stringArray1[i], stringArray2[i], stringArray3[i], stringArray4[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Clear memory */ - delete [] stringArray0; - delete [] stringArray1; - delete [] stringArray2; - delete [] stringArray3; - delete [] stringArray4; - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedConfigurationTriggerGet : Read the event configuration - * - * - Parameters : - * int SocketIndex - * char *EventTriggerConfiguration - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedConfigurationTriggerGet (int SocketIndex, char * EventTriggerConfiguration) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedConfigurationTriggerGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (EventTriggerConfiguration, pt); - ptNext = strchr (EventTriggerConfiguration, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedConfigurationActionSet : Configure one or several actions - * - * - Parameters : - * int SocketIndex - * int nbElement - * char *ExtendedActionName - * char *ActionParameter1 - * char *ActionParameter2 - * char *ActionParameter3 - * char *ActionParameter4 - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedConfigurationActionSet (int SocketIndex, int NbElements, char * ExtendedActionNameList, char * ActionParameter1List, char * ActionParameter2List, char * ActionParameter3List, char * ActionParameter4List) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Split list */ - char *token; - char seps[] = " \t;"; - int indice; - char list [SIZE_NOMINAL]; - char *list_r; - char subString[] = "{}"; - - char (*stringArray0)[SIZE_NAME]; - stringArray0 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, ExtendedActionNameList, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray0[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray0[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - char (*stringArray1)[SIZE_NAME]; - stringArray1 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, ActionParameter1List, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray1[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray1[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray1[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray1[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - char (*stringArray2)[SIZE_NAME]; - stringArray2 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, ActionParameter2List, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray2[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray2[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray2[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray2[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - char (*stringArray3)[SIZE_NAME]; - stringArray3 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, ActionParameter3List, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray3[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray3[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray3[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray3[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - char (*stringArray4)[SIZE_NAME]; - stringArray4 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, ActionParameter4List, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray4[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray4[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray4[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray4[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedConfigurationActionSet ("); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%s,%s,%s,%s,%s", stringArray0[i], stringArray1[i], stringArray2[i], stringArray3[i], stringArray4[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Clear memory */ - delete [] stringArray0; - delete [] stringArray1; - delete [] stringArray2; - delete [] stringArray3; - delete [] stringArray4; - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedConfigurationActionGet : Read the action configuration - * - * - Parameters : - * int SocketIndex - * char *ActionConfiguration - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedConfigurationActionGet (int SocketIndex, char * ActionConfiguration) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedConfigurationActionGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ActionConfiguration, pt); - ptNext = strchr (ActionConfiguration, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedStart : Launch the last event and action configuration and return an ID - * - * - Parameters : - * int SocketIndex - * int *ID - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedStart (int SocketIndex, int * ID) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedStart (int *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", ID); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedAllGet : Read all event and action configurations - * - * - Parameters : - * int SocketIndex - * char *EventActionConfigurations - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedAllGet (int SocketIndex, char * EventActionConfigurations) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedAllGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (EventActionConfigurations, pt); - ptNext = strchr (EventActionConfigurations, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedGet : Read the event and action configuration defined by ID - * - * - Parameters : - * int SocketIndex - * int ID - * char *EventTriggerConfiguration - * char *ActionConfiguration - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedGet (int SocketIndex, int ID, char * EventTriggerConfiguration, char * ActionConfiguration) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedGet (%d,char *,char *)", ID); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (EventTriggerConfiguration, pt); - ptNext = strchr (EventTriggerConfiguration, ','); - if (ptNext != NULL) *ptNext = '\0'; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ActionConfiguration, pt); - ptNext = strchr (ActionConfiguration, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedRemove : Remove the event and action configuration defined by ID - * - * - Parameters : - * int SocketIndex - * int ID - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedRemove (int SocketIndex, int ID) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedRemove (%d)", ID); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventExtendedWait : Wait events from the last event configuration - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventExtendedWait (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventExtendedWait ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringConfigurationGet : Read different mnemonique type - * - * - Parameters : - * int SocketIndex - * char *Type - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringConfigurationGet (int SocketIndex, char * Type) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringConfigurationGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (Type, pt); - ptNext = strchr (Type, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringConfigurationSet : Configuration acquisition - * - * - Parameters : - * int SocketIndex - * int nbElement - * char *Type - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringConfigurationSet (int SocketIndex, int NbElements, char * TypeList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Split list */ - char *token; - char seps[] = " \t;"; - int indice; - char list [SIZE_NOMINAL]; - char *list_r; - char subString[] = "{}"; - - char (*stringArray0)[SIZE_NAME]; - stringArray0 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, TypeList, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray0[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray0[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringConfigurationSet ("); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%s", stringArray0[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Clear memory */ - delete [] stringArray0; - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringCurrentNumberGet : Maximum number of samples and current number during acquisition - * - * - Parameters : - * int SocketIndex - * int *CurrentNumber - * int *MaximumSamplesNumber - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringCurrentNumberGet (int SocketIndex, int * CurrentNumber, int * MaximumSamplesNumber) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringCurrentNumberGet (int *,int *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", CurrentNumber); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", MaximumSamplesNumber); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringStopAndSave : Stop acquisition and save data - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringStopAndSave (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringStopAndSave ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringDataAcquire : Acquire a configured data - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringDataAcquire (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringDataAcquire ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringDataGet : Get a data line from gathering buffer - * - * - Parameters : - * int SocketIndex - * int IndexPoint - * char *DataBufferLine - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringDataGet (int SocketIndex, int IndexPoint, char * DataBufferLine) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringDataGet (%d,char *)", IndexPoint); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (DataBufferLine, pt); - ptNext = strchr (DataBufferLine, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringReset : Empty the gathered data in memory to start new gathering from scratch - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringReset (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringReset ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringRun : Start a new gathering - * - * - Parameters : - * int SocketIndex - * int DataNumber - * int Divisor - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringRun (int SocketIndex, int DataNumber, int Divisor) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringRun (%d,%d)", DataNumber, Divisor); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringStop : Stop the data gathering (without saving to file) - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringStop (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringStop ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringExternalConfigurationSet : Configuration acquisition - * - * - Parameters : - * int SocketIndex - * int nbElement - * char *Type - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringExternalConfigurationSet (int SocketIndex, int NbElements, char * TypeList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Split list */ - char *token; - char seps[] = " \t;"; - int indice; - char list [SIZE_NOMINAL]; - char *list_r; - char subString[] = "{}"; - - char (*stringArray0)[SIZE_NAME]; - stringArray0 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, TypeList, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray0[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray0[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringExternalConfigurationSet ("); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%s", stringArray0[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Clear memory */ - delete [] stringArray0; - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringExternalConfigurationGet : Read different mnemonique type - * - * - Parameters : - * int SocketIndex - * char *Type - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringExternalConfigurationGet (int SocketIndex, char * Type) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringExternalConfigurationGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (Type, pt); - ptNext = strchr (Type, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringExternalCurrentNumberGet : Maximum number of samples and current number during acquisition - * - * - Parameters : - * int SocketIndex - * int *CurrentNumber - * int *MaximumSamplesNumber - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringExternalCurrentNumberGet (int SocketIndex, int * CurrentNumber, int * MaximumSamplesNumber) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringExternalCurrentNumberGet (int *,int *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", CurrentNumber); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", MaximumSamplesNumber); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringExternalStopAndSave : Stop acquisition and save data - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringExternalStopAndSave (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringExternalStopAndSave ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GlobalArrayGet : Get global array value - * - * - Parameters : - * int SocketIndex - * int Number - * char *ValueString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGlobalArrayGet (int SocketIndex, int Number, char * ValueString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GlobalArrayGet (%d,char *)", Number); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ValueString, pt); - ptNext = strchr (ValueString, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GlobalArraySet : Set global array value - * - * - Parameters : - * int SocketIndex - * int Number - * char *ValueString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGlobalArraySet (int SocketIndex, int Number, char * ValueString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GlobalArraySet (%d,%s)", Number, ValueString); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * DoubleGlobalArrayGet : Get double global array value - * - * - Parameters : - * int SocketIndex - * int Number - * double *DoubleValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPDoubleGlobalArrayGet (int SocketIndex, int Number, double * DoubleValue) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "DoubleGlobalArrayGet (%d,double *)", Number); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", DoubleValue); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * DoubleGlobalArraySet : Set double global array value - * - * - Parameters : - * int SocketIndex - * int Number - * double DoubleValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPDoubleGlobalArraySet (int SocketIndex, int Number, double DoubleValue) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "DoubleGlobalArraySet (%d,%.13g)", Number, DoubleValue); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GPIOAnalogGet : Read analog input or analog output for one or few input - * - * - Parameters : - * int SocketIndex - * int nbElement - * char *GPIOName - * double *AnalogValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGPIOAnalogGet (int SocketIndex, int NbElements, char * GPIONameList, double AnalogValue[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Split list */ - char *token; - char seps[] = " \t;"; - int indice; - char list [SIZE_NOMINAL]; - char *list_r; - char subString[] = "{}"; - - char (*stringArray0)[SIZE_NAME]; - stringArray0 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, GPIONameList, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray0[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray0[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - - /* Convert to string */ - sprintf (ExecuteMethod, "GPIOAnalogGet ("); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%s,double *", stringArray0[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Clear memory */ - delete [] stringArray0; - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - - for (int i = 0; i < NbElements; i++) - { - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", &AnalogValue[i]); - } - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GPIOAnalogSet : Set analog output for one or few output - * - * - Parameters : - * int SocketIndex - * int nbElement - * char *GPIOName - * double AnalogOutputValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGPIOAnalogSet (int SocketIndex, int NbElements, char * GPIONameList, double AnalogOutputValue[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Split list */ - char *token; - char seps[] = " \t;"; - int indice; - char list [SIZE_NOMINAL]; - char *list_r; - char subString[] = "{}"; - - char (*stringArray0)[SIZE_NAME]; - stringArray0 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, GPIONameList, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray0[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray0[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - - /* Convert to string */ - sprintf (ExecuteMethod, "GPIOAnalogSet ("); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%s,%.13g", stringArray0[i], AnalogOutputValue[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Clear memory */ - delete [] stringArray0; - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GPIOAnalogGainGet : Read analog input gain (1, 2, 4 or 8) for one or few input - * - * - Parameters : - * int SocketIndex - * int nbElement - * char *GPIOName - * int *AnalogInputGainValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGPIOAnalogGainGet (int SocketIndex, int NbElements, char * GPIONameList, int AnalogInputGainValue[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Split list */ - char *token; - char seps[] = " \t;"; - int indice; - char list [SIZE_NOMINAL]; - char *list_r; - char subString[] = "{}"; - - char (*stringArray0)[SIZE_NAME]; - stringArray0 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, GPIONameList, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray0[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray0[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - - /* Convert to string */ - sprintf (ExecuteMethod, "GPIOAnalogGainGet ("); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%s,int *", stringArray0[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Clear memory */ - delete [] stringArray0; - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - - for (int i = 0; i < NbElements; i++) - { - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", &AnalogInputGainValue[i]); - } - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GPIOAnalogGainSet : Set analog input gain (1, 2, 4 or 8) for one or few input - * - * - Parameters : - * int SocketIndex - * int nbElement - * char *GPIOName - * int AnalogInputGainValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGPIOAnalogGainSet (int SocketIndex, int NbElements, char * GPIONameList, int AnalogInputGainValue[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Split list */ - char *token; - char seps[] = " \t;"; - int indice; - char list [SIZE_NOMINAL]; - char *list_r; - char subString[] = "{}"; - - char (*stringArray0)[SIZE_NAME]; - stringArray0 = new char [NbElements][SIZE_NAME]; - indice = 0; - strncpyWithEOS(list, GPIONameList, SIZE_NOMINAL, SIZE_NOMINAL); - ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ - CleanString(list, '{', '}'); - list_r = NULL; - token = strtok_r (list, seps, &list_r); - while ((NULL != token) && (indice < NbElements)) - { - memset(stringArray0[indice],'\0', SIZE_NAME); - strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); - ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); - DeleteCharacters (stringArray0[indice], subString); - token = strtok_r (NULL, seps, &list_r); - indice++; - } - - /* Convert to string */ - sprintf (ExecuteMethod, "GPIOAnalogGainSet ("); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%s,%d", stringArray0[i], AnalogInputGainValue[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Clear memory */ - delete [] stringArray0; - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GPIODigitalGet : Read digital output or digital input - * - * - Parameters : - * int SocketIndex - * char *GPIOName - * unsigned short *DigitalValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGPIODigitalGet (int SocketIndex, char * GPIOName, unsigned short * DigitalValue) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GPIODigitalGet (%s,unsigned short *)", GPIOName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%hu", DigitalValue); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GPIODigitalSet : Set Digital Output for one or few output TTL - * - * - Parameters : - * int SocketIndex - * char *GPIOName - * unsigned short Mask - * unsigned short DigitalOutputValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGPIODigitalSet (int SocketIndex, char * GPIOName, unsigned short Mask, unsigned short DigitalOutputValue) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GPIODigitalSet (%s,%hu,%hu)", GPIOName, Mask, DigitalOutputValue); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupCorrectorOutputGet : Return corrector outputs - * - * - Parameters : - * int SocketIndex - * char *GroupName - * int nbElement - * double *CorrectorOutput - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupCorrectorOutputGet (int SocketIndex, char * GroupName, int NbElements, double CorrectorOutput[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupCorrectorOutputGet (%s,", GroupName); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "double *"); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - - for (int i = 0; i < NbElements; i++) - { - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", &CorrectorOutput[i]); - } - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupHomeSearch : Start home search sequence - * - * - Parameters : - * int SocketIndex - * char *GroupName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupHomeSearch (int SocketIndex, char * GroupName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupHomeSearch (%s)", GroupName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupHomeSearchAndRelativeMove : Start home search sequence and execute a displacement - * - * - Parameters : - * int SocketIndex - * char *GroupName - * double TargetDisplacement - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupHomeSearchAndRelativeMove (int SocketIndex, char * GroupName, int NbElements, double TargetDisplacement[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupHomeSearchAndRelativeMove (%s,", GroupName); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%.13g", TargetDisplacement[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupInitialize : Start the initialization - * - * - Parameters : - * int SocketIndex - * char *GroupName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupInitialize (int SocketIndex, char * GroupName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupInitialize (%s)", GroupName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupInitializeWithEncoderCalibration : Start the initialization with encoder calibration - * - * - Parameters : - * int SocketIndex - * char *GroupName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupInitializeWithEncoderCalibration (int SocketIndex, char * GroupName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupInitializeWithEncoderCalibration (%s)", GroupName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupKill : Kill the group - * - * - Parameters : - * int SocketIndex - * char *GroupName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupKill (int SocketIndex, char * GroupName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupKill (%s)", GroupName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupMoveAbort : Abort a move - * - * - Parameters : - * int SocketIndex - * char *GroupName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupMoveAbort (int SocketIndex, char * GroupName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupMoveAbort (%s)", GroupName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupMoveAbsolute : Do an absolute move - * - * - Parameters : - * int SocketIndex - * char *GroupName - * double TargetPosition - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupMoveAbsolute (int SocketIndex, char * GroupName, int NbElements, double TargetPosition[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupMoveAbsolute (%s,", GroupName); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%.13g", TargetPosition[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupMoveRelative : Do a relative move - * - * - Parameters : - * int SocketIndex - * char *GroupName - * double TargetDisplacement - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupMoveRelative (int SocketIndex, char * GroupName, int NbElements, double TargetDisplacement[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupMoveRelative (%s,", GroupName); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "%.13g", TargetDisplacement[i]); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupMotionDisable : Set Motion disable on selected group - * - * - Parameters : - * int SocketIndex - * char *GroupName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupMotionDisable (int SocketIndex, char * GroupName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupMotionDisable (%s)", GroupName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupMotionEnable : Set Motion enable on selected group - * - * - Parameters : - * int SocketIndex - * char *GroupName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupMotionEnable (int SocketIndex, char * GroupName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupMotionEnable (%s)", GroupName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupPositionCorrectedProfilerGet : Return corrected profiler positions - * - * - Parameters : - * int SocketIndex - * char *GroupName - * double PositionX - * double PositionY - * double *CorrectedProfilerPositionX - * double *CorrectedProfilerPositionY - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupPositionCorrectedProfilerGet (int SocketIndex, char * GroupName, double PositionX, double PositionY, double * CorrectedProfilerPositionX, double * CorrectedProfilerPositionY) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupPositionCorrectedProfilerGet (%s,%.13g,%.13g,double *,double *)", GroupName, PositionX, PositionY); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CorrectedProfilerPositionX); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CorrectedProfilerPositionY); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupPositionCurrentGet : Return current positions - * - * - Parameters : - * int SocketIndex - * char *GroupName - * int nbElement - * double *CurrentEncoderPosition - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupPositionCurrentGet (int SocketIndex, char * GroupName, int NbElements, double CurrentEncoderPosition[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupPositionCurrentGet (%s,", GroupName); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "double *"); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - - for (int i = 0; i < NbElements; i++) - { - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", &CurrentEncoderPosition[i]); - } - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupPositionSetpointGet : Return setpoint positions - * - * - Parameters : - * int SocketIndex - * char *GroupName - * int nbElement - * double *SetPointPosition - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupPositionSetpointGet (int SocketIndex, char * GroupName, int NbElements, double SetPointPosition[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupPositionSetpointGet (%s,", GroupName); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "double *"); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - - for (int i = 0; i < NbElements; i++) - { - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", &SetPointPosition[i]); - } - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupPositionTargetGet : Return target positions - * - * - Parameters : - * int SocketIndex - * char *GroupName - * int nbElement - * double *TargetPosition - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupPositionTargetGet (int SocketIndex, char * GroupName, int NbElements, double TargetPosition[]) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - char temp[SIZE_NOMINAL]; - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupPositionTargetGet (%s,", GroupName); - for (int i = 0; i < NbElements; i++) - { - sprintf (temp, "double *"); - strncat (ExecuteMethod, temp, SIZE_SMALL); - if ((i + 1) < NbElements) - { - strncat (ExecuteMethod, ",", SIZE_SMALL); - } - } - strcat (ExecuteMethod, ")"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - - for (int i = 0; i < NbElements; i++) - { - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", &TargetPosition[i]); - } - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupStatusGet : Return group status - * - * - Parameters : - * int SocketIndex - * char *GroupName - * int *Status - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupStatusGet (int SocketIndex, char * GroupName, int * Status) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupStatusGet (%s,int *)", GroupName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", Status); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupStatusStringGet : Return the group status string corresponding to the group status code - * - * - Parameters : - * int SocketIndex - * int GroupStatusCode - * char *GroupStatusString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupStatusStringGet (int SocketIndex, int GroupStatusCode, char * GroupStatusString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupStatusStringGet (%d,char *)", GroupStatusCode); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (GroupStatusString, pt); - ptNext = strchr (GroupStatusString, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * KillAll : Put all groups in 'Not initialized' state - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPKillAll (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "KillAll ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * RestartApplication : Restart the Controller - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPRestartApplication (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "RestartApplication ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerBacklashGet : Read backlash value and status - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *BacklashValue - * char *BacklaskStatus - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerBacklashGet (int SocketIndex, char * PositionerName, double * BacklashValue, char * BacklaskStatus) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerBacklashGet (%s,double *,char *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", BacklashValue); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (BacklaskStatus, pt); - ptNext = strchr (BacklaskStatus, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerBacklashSet : Set backlash value - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double BacklashValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerBacklashSet (int SocketIndex, char * PositionerName, double BacklashValue) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerBacklashSet (%s,%.13g)", PositionerName, BacklashValue); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerBacklashEnable : Enable the backlash - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerBacklashEnable (int SocketIndex, char * PositionerName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerBacklashEnable (%s)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerBacklashDisable : Disable the backlash - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerBacklashDisable (int SocketIndex, char * PositionerName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerBacklashDisable (%s)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorNotchFiltersSet : Update filters parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double NotchFrequency1 - * double NotchBandwith1 - * double NotchGain1 - * double NotchFrequency2 - * double NotchBandwith2 - * double NotchGain2 - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorNotchFiltersSet (int SocketIndex, char * PositionerName, double NotchFrequency1, double NotchBandwith1, double NotchGain1, double NotchFrequency2, double NotchBandwith2, double NotchGain2) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorNotchFiltersSet (%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, NotchFrequency1, NotchBandwith1, NotchGain1, NotchFrequency2, NotchBandwith2, NotchGain2); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorNotchFiltersGet : Read filters parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *NotchFrequency1 - * double *NotchBandwith1 - * double *NotchGain1 - * double *NotchFrequency2 - * double *NotchBandwith2 - * double *NotchGain2 - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorNotchFiltersGet (int SocketIndex, char * PositionerName, double * NotchFrequency1, double * NotchBandwith1, double * NotchGain1, double * NotchFrequency2, double * NotchBandwith2, double * NotchGain2) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorNotchFiltersGet (%s,double *,double *,double *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", NotchFrequency1); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", NotchBandwith1); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", NotchGain1); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", NotchFrequency2); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", NotchBandwith2); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", NotchGain2); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorPIDFFAccelerationSet : Update corrector parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * bool ClosedLoopStatus - * double KP - * double KI - * double KD - * double KS - * double IntegrationTime - * double DerivativeFilterCutOffFrequency - * double GKP - * double GKI - * double GKD - * double KForm - * double FeedForwardGainAcceleration - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorPIDFFAccelerationSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainAcceleration) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorPIDFFAccelerationSet (%s,%d,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, ClosedLoopStatus, KP, KI, KD, KS, IntegrationTime, DerivativeFilterCutOffFrequency, GKP, GKI, GKD, KForm, FeedForwardGainAcceleration); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorPIDFFAccelerationGet : Read corrector parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * bool *ClosedLoopStatus - * double *KP - * double *KI - * double *KD - * double *KS - * double *IntegrationTime - * double *DerivativeFilterCutOffFrequency - * double *GKP - * double *GKI - * double *GKD - * double *KForm - * double *FeedForwardGainAcceleration - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorPIDFFAccelerationGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainAcceleration) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - int boolScanTmp; - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorPIDFFAccelerationGet (%s,bool *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", &boolScanTmp); - *ClosedLoopStatus = (bool) boolScanTmp; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KP); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KI); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KD); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KS); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", IntegrationTime); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", DerivativeFilterCutOffFrequency); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKP); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKI); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKD); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KForm); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", FeedForwardGainAcceleration); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorPIDFFVelocitySet : Update corrector parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * bool ClosedLoopStatus - * double KP - * double KI - * double KD - * double KS - * double IntegrationTime - * double DerivativeFilterCutOffFrequency - * double GKP - * double GKI - * double GKD - * double KForm - * double FeedForwardGainVelocity - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorPIDFFVelocitySet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainVelocity) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorPIDFFVelocitySet (%s,%d,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, ClosedLoopStatus, KP, KI, KD, KS, IntegrationTime, DerivativeFilterCutOffFrequency, GKP, GKI, GKD, KForm, FeedForwardGainVelocity); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorPIDFFVelocityGet : Read corrector parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * bool *ClosedLoopStatus - * double *KP - * double *KI - * double *KD - * double *KS - * double *IntegrationTime - * double *DerivativeFilterCutOffFrequency - * double *GKP - * double *GKI - * double *GKD - * double *KForm - * double *FeedForwardGainVelocity - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorPIDFFVelocityGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainVelocity) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - int boolScanTmp; - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorPIDFFVelocityGet (%s,bool *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", &boolScanTmp); - *ClosedLoopStatus = (bool) boolScanTmp; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KP); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KI); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KD); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KS); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", IntegrationTime); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", DerivativeFilterCutOffFrequency); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKP); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKI); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKD); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KForm); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", FeedForwardGainVelocity); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorPIDDualFFVoltageSet : Update corrector parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * bool ClosedLoopStatus - * double KP - * double KI - * double KD - * double KS - * double IntegrationTime - * double DerivativeFilterCutOffFrequency - * double GKP - * double GKI - * double GKD - * double KForm - * double FeedForwardGainVelocity - * double FeedForwardGainAcceleration - * double Friction - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorPIDDualFFVoltageSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainVelocity, double FeedForwardGainAcceleration, double Friction) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorPIDDualFFVoltageSet (%s,%d,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, ClosedLoopStatus, KP, KI, KD, KS, IntegrationTime, DerivativeFilterCutOffFrequency, GKP, GKI, GKD, KForm, FeedForwardGainVelocity, FeedForwardGainAcceleration, Friction); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorPIDDualFFVoltageGet : Read corrector parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * bool *ClosedLoopStatus - * double *KP - * double *KI - * double *KD - * double *KS - * double *IntegrationTime - * double *DerivativeFilterCutOffFrequency - * double *GKP - * double *GKI - * double *GKD - * double *KForm - * double *FeedForwardGainVelocity - * double *FeedForwardGainAcceleration - * double *Friction - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorPIDDualFFVoltageGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainVelocity, double * FeedForwardGainAcceleration, double * Friction) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - int boolScanTmp; - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorPIDDualFFVoltageGet (%s,bool *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", &boolScanTmp); - *ClosedLoopStatus = (bool) boolScanTmp; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KP); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KI); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KD); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KS); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", IntegrationTime); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", DerivativeFilterCutOffFrequency); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKP); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKI); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", GKD); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KForm); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", FeedForwardGainVelocity); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", FeedForwardGainAcceleration); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Friction); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorPIPositionSet : Update corrector parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * bool ClosedLoopStatus - * double KP - * double KI - * double IntegrationTime - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorPIPositionSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double IntegrationTime) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorPIPositionSet (%s,%d,%.13g,%.13g,%.13g)", PositionerName, ClosedLoopStatus, KP, KI, IntegrationTime); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorPIPositionGet : Read corrector parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * bool *ClosedLoopStatus - * double *KP - * double *KI - * double *IntegrationTime - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorPIPositionGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * IntegrationTime) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - int boolScanTmp; - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorPIPositionGet (%s,bool *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", &boolScanTmp); - *ClosedLoopStatus = (bool) boolScanTmp; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KP); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", KI); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", IntegrationTime); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCorrectorTypeGet : Read corrector type - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * char *CorrectorType - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCorrectorTypeGet (int SocketIndex, char * PositionerName, char * CorrectorType) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCorrectorTypeGet (%s,char *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (CorrectorType, pt); - ptNext = strchr (CorrectorType, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCurrentVelocityAccelerationFiltersSet : Set current velocity and acceleration cut off frequencies - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double CurrentVelocityCutOffFrequency - * double CurrentAccelerationCutOffFrequency - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCurrentVelocityAccelerationFiltersSet (int SocketIndex, char * PositionerName, double CurrentVelocityCutOffFrequency, double CurrentAccelerationCutOffFrequency) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCurrentVelocityAccelerationFiltersSet (%s,%.13g,%.13g)", PositionerName, CurrentVelocityCutOffFrequency, CurrentAccelerationCutOffFrequency); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerCurrentVelocityAccelerationFiltersGet : Get current velocity and acceleration cut off frequencies - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *CurrentVelocityCutOffFrequency - * double *CurrentAccelerationCutOffFrequency - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerCurrentVelocityAccelerationFiltersGet (int SocketIndex, char * PositionerName, double * CurrentVelocityCutOffFrequency, double * CurrentAccelerationCutOffFrequency) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerCurrentVelocityAccelerationFiltersGet (%s,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CurrentVelocityCutOffFrequency); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CurrentAccelerationCutOffFrequency); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerDriverStatusGet : Read positioner driver status - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * int *DriverStatus - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerDriverStatusGet (int SocketIndex, char * PositionerName, int * DriverStatus) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerDriverStatusGet (%s,int *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", DriverStatus); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerDriverStatusStringGet : Return the positioner driver status string corresponding to the positioner error code - * - * - Parameters : - * int SocketIndex - * int PositionerDriverStatus - * char *PositionerDriverStatusString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerDriverStatusStringGet (int SocketIndex, int PositionerDriverStatus, char * PositionerDriverStatusString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerDriverStatusStringGet (%d,char *)", PositionerDriverStatus); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (PositionerDriverStatusString, pt); - ptNext = strchr (PositionerDriverStatusString, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerEncoderAmplitudeValuesGet : Read analog interpolated encoder amplitude values - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *CalibrationSinusAmplitude - * double *CurrentSinusAmplitude - * double *CalibrationCosinusAmplitude - * double *CurrentCosinusAmplitude - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerEncoderAmplitudeValuesGet (int SocketIndex, char * PositionerName, double * CalibrationSinusAmplitude, double * CurrentSinusAmplitude, double * CalibrationCosinusAmplitude, double * CurrentCosinusAmplitude) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerEncoderAmplitudeValuesGet (%s,double *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CalibrationSinusAmplitude); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CurrentSinusAmplitude); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CalibrationCosinusAmplitude); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CurrentCosinusAmplitude); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerEncoderCalibrationParametersGet : Read analog interpolated encoder calibration parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *SinusOffset - * double *CosinusOffset - * double *DifferentialGain - * double *PhaseCompensation - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerEncoderCalibrationParametersGet (int SocketIndex, char * PositionerName, double * SinusOffset, double * CosinusOffset, double * DifferentialGain, double * PhaseCompensation) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerEncoderCalibrationParametersGet (%s,double *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SinusOffset); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CosinusOffset); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", DifferentialGain); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", PhaseCompensation); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerErrorGet : Read and clear positioner error code - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * int *ErrorCode - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerErrorGet (int SocketIndex, char * PositionerName, int * ErrorCode) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerErrorGet (%s,int *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", ErrorCode); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerErrorRead : Read only positioner error code without clear it - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * int *ErrorCode - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerErrorRead (int SocketIndex, char * PositionerName, int * ErrorCode) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerErrorRead (%s,int *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", ErrorCode); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerErrorStringGet : Return the positioner status string corresponding to the positioner error code - * - * - Parameters : - * int SocketIndex - * int PositionerErrorCode - * char *PositionerErrorString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerErrorStringGet (int SocketIndex, int PositionerErrorCode, char * PositionerErrorString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerErrorStringGet (%d,char *)", PositionerErrorCode); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (PositionerErrorString, pt); - ptNext = strchr (PositionerErrorString, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerHardwareStatusGet : Read positioner hardware status - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * int *HardwareStatus - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerHardwareStatusGet (int SocketIndex, char * PositionerName, int * HardwareStatus) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerHardwareStatusGet (%s,int *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", HardwareStatus); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerHardwareStatusStringGet : Return the positioner hardware status string corresponding to the positioner error code - * - * - Parameters : - * int SocketIndex - * int PositionerHardwareStatus - * char *PositionerHardwareStatusString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerHardwareStatusStringGet (int SocketIndex, int PositionerHardwareStatus, char * PositionerHardwareStatusString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerHardwareStatusStringGet (%d,char *)", PositionerHardwareStatus); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (PositionerHardwareStatusString, pt); - ptNext = strchr (PositionerHardwareStatusString, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerHardInterpolatorFactorGet : Get hard interpolator parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * int *InterpolationFactor - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerHardInterpolatorFactorGet (int SocketIndex, char * PositionerName, int * InterpolationFactor) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerHardInterpolatorFactorGet (%s,int *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", InterpolationFactor); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerHardInterpolatorFactorSet : Set hard interpolator parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * int InterpolationFactor - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerHardInterpolatorFactorSet (int SocketIndex, char * PositionerName, int InterpolationFactor) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerHardInterpolatorFactorSet (%s,%d)", PositionerName, InterpolationFactor); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerMaximumVelocityAndAccelerationGet : Return maximum velocity and acceleration of the positioner - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *MaximumVelocity - * double *MaximumAcceleration - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerMaximumVelocityAndAccelerationGet (int SocketIndex, char * PositionerName, double * MaximumVelocity, double * MaximumAcceleration) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerMaximumVelocityAndAccelerationGet (%s,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MaximumVelocity); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MaximumAcceleration); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerMotionDoneGet : Read motion done parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *PositionWindow - * double *VelocityWindow - * double *CheckingTime - * double *MeanPeriod - * double *TimeOut - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerMotionDoneGet (int SocketIndex, char * PositionerName, double * PositionWindow, double * VelocityWindow, double * CheckingTime, double * MeanPeriod, double * TimeOut) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerMotionDoneGet (%s,double *,double *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", PositionWindow); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", VelocityWindow); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CheckingTime); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MeanPeriod); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", TimeOut); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerMotionDoneSet : Update motion done parameters - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double PositionWindow - * double VelocityWindow - * double CheckingTime - * double MeanPeriod - * double TimeOut - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerMotionDoneSet (int SocketIndex, char * PositionerName, double PositionWindow, double VelocityWindow, double CheckingTime, double MeanPeriod, double TimeOut) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerMotionDoneSet (%s,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, PositionWindow, VelocityWindow, CheckingTime, MeanPeriod, TimeOut); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerSGammaExactVelocityAjustedDisplacementGet : Return adjusted displacement to get exact velocity - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double DesiredDisplacement - * double *AdjustedDisplacement - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerSGammaExactVelocityAjustedDisplacementGet (int SocketIndex, char * PositionerName, double DesiredDisplacement, double * AdjustedDisplacement) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerSGammaExactVelocityAjustedDisplacementGet (%s,%.13g,double *)", PositionerName, DesiredDisplacement); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", AdjustedDisplacement); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerSGammaParametersGet : Read dynamic parameters for one axe of a group for a future displacement - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *Velocity - * double *Acceleration - * double *MinimumTjerkTime - * double *MaximumTjerkTime - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerSGammaParametersGet (int SocketIndex, char * PositionerName, double * Velocity, double * Acceleration, double * MinimumTjerkTime, double * MaximumTjerkTime) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerSGammaParametersGet (%s,double *,double *,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Velocity); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Acceleration); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MinimumTjerkTime); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MaximumTjerkTime); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerSGammaParametersSet : Update dynamic parameters for one axe of a group for a future displacement - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double Velocity - * double Acceleration - * double MinimumTjerkTime - * double MaximumTjerkTime - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerSGammaParametersSet (int SocketIndex, char * PositionerName, double Velocity, double Acceleration, double MinimumTjerkTime, double MaximumTjerkTime) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerSGammaParametersSet (%s,%.13g,%.13g,%.13g,%.13g)", PositionerName, Velocity, Acceleration, MinimumTjerkTime, MaximumTjerkTime); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerSGammaPreviousMotionTimesGet : Read SettingTime and SettlingTime - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *SettingTime - * double *SettlingTime - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerSGammaPreviousMotionTimesGet (int SocketIndex, char * PositionerName, double * SettingTime, double * SettlingTime) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerSGammaPreviousMotionTimesGet (%s,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SettingTime); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SettlingTime); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerStageParameterGet : Return the stage parameter - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * char *ParameterName - * char *ParameterValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerStageParameterGet (int SocketIndex, char * PositionerName, char * ParameterName, char * ParameterValue) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerStageParameterGet (%s,%s,char *)", PositionerName, ParameterName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ParameterValue, pt); - ptNext = strchr (ParameterValue, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerStageParameterSet : Save the stage parameter - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * char *ParameterName - * char *ParameterValue - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerStageParameterSet (int SocketIndex, char * PositionerName, char * ParameterName, char * ParameterValue) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerStageParameterSet (%s,%s,%s)", PositionerName, ParameterName, ParameterValue); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerUserTravelLimitsGet : Read UserMinimumTarget and UserMaximumTarget - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double *UserMinimumTarget - * double *UserMaximumTarget - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerUserTravelLimitsGet (int SocketIndex, char * PositionerName, double * UserMinimumTarget, double * UserMaximumTarget) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerUserTravelLimitsGet (%s,double *,double *)", PositionerName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserMinimumTarget); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserMaximumTarget); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerUserTravelLimitsSet : Update UserMinimumTarget and UserMaximumTarget - * - * - Parameters : - * int SocketIndex - * char *PositionerName - * double UserMinimumTarget - * double UserMaximumTarget - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerUserTravelLimitsSet (int SocketIndex, char * PositionerName, double UserMinimumTarget, double UserMaximumTarget) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerUserTravelLimitsSet (%s,%.13g,%.13g)", PositionerName, UserMinimumTarget, UserMaximumTarget); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * HexapodMoveAbsolute : Hexapod absolute move in a specific coordinate system - * - * - Parameters : - * int SocketIndex - * char *GroupName - * char *CoordinateSystem - * double X - * double Y - * double Z - * double U - * double V - * double W - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPHexapodMoveAbsolute (int SocketIndex, char * GroupName, char * CoordinateSystem, double X, double Y, double Z, double U, double V, double W) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "HexapodMoveAbsolute (%s,%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", GroupName, CoordinateSystem, X, Y, Z, U, V, W); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * HexapodMoveIncremental : Hexapod incremental move in a specific coordinate system - * - * - Parameters : - * int SocketIndex - * char *GroupName - * char *CoordinateSystem - * double dX - * double dY - * double dZ - * double dU - * double dV - * double dW - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPHexapodMoveIncremental (int SocketIndex, char * GroupName, char * CoordinateSystem, double dX, double dY, double dZ, double dU, double dV, double dW) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "HexapodMoveIncremental (%s,%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", GroupName, CoordinateSystem, dX, dY, dZ, dU, dV, dW); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * HexapodCoordinatesGet : Get coordinates in a specific coordinate system of a point specified in another coordinate system - * - * - Parameters : - * int SocketIndex - * char *GroupName - * char *CoordinateSystemIn - * char *CoordinateSystemOut - * double Xin - * double Yin - * double Zin - * double Uin - * double Vin - * double Win - * double *Xout - * double *Yout - * double *Zout - * double *Uout - * double *Vout - * double *Wout - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPHexapodCoordinatesGet (int SocketIndex, char * GroupName, char * CoordinateSystemIn, char * CoordinateSystemOut, double Xin, double Yin, double Zin, double Uin, double Vin, double Win, double * Xout, double * Yout, double * Zout, double * Uout, double * Vout, double * Wout) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "HexapodCoordinatesGet (%s,%s,%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,double *,double *,double *,double *,double *,double *)", GroupName, CoordinateSystemIn, CoordinateSystemOut, Xin, Yin, Zin, Uin, Vin, Win); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Xout); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Yout); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Zout); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Uout); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Vout); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Wout); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * HexapodCoordinateSystemSet : Modify the position of a coordinate system - * - * - Parameters : - * int SocketIndex - * char *GroupName - * char *CoordinateSystem - * double X - * double Y - * double Z - * double U - * double V - * double W - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPHexapodCoordinateSystemSet (int SocketIndex, char * GroupName, char * CoordinateSystem, double X, double Y, double Z, double U, double V, double W) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "HexapodCoordinateSystemSet (%s,%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", GroupName, CoordinateSystem, X, Y, Z, U, V, W); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * HexapodCoordinateSystemGet : Get the position of a coordinate system - * - * - Parameters : - * int SocketIndex - * char *GroupName - * char *CoordinateSystem - * double *X - * double *Y - * double *Z - * double *U - * double *V - * double *W - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPHexapodCoordinateSystemGet (int SocketIndex, char * GroupName, char * CoordinateSystem, double * X, double * Y, double * Z, double * U, double * V, double * W) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "HexapodCoordinateSystemGet (%s,%s,double *,double *,double *,double *,double *,double *)", GroupName, CoordinateSystem); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", X); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Y); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", Z); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", U); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", V); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", W); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * OptionalModuleExecute : Execute an optional module - * - * - Parameters : - * int SocketIndex - * char *ModuleFileName - * char *TaskName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPOptionalModuleExecute (int SocketIndex, char * ModuleFileName, char * TaskName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "OptionalModuleExecute (%s,%s)", ModuleFileName, TaskName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * OptionalModuleKill : Kill an optional module - * - * - Parameters : - * int SocketIndex - * char *TaskName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPOptionalModuleKill (int SocketIndex, char * TaskName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "OptionalModuleKill (%s)", TaskName); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ControllerStatusGet : Read controller current status - * - * - Parameters : - * int SocketIndex - * int *ControllerStatus - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPControllerStatusGet (int SocketIndex, int * ControllerStatus) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "ControllerStatusGet (int *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%d", ControllerStatus); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ControllerStatusStringGet : Return the controller status string corresponding to the controller status code - * - * - Parameters : - * int SocketIndex - * int ControllerStatusCode - * char *ControllerStatusString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPControllerStatusStringGet (int SocketIndex, int ControllerStatusCode, char * ControllerStatusString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "ControllerStatusStringGet (%d,char *)", ControllerStatusCode); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ControllerStatusString, pt); - ptNext = strchr (ControllerStatusString, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EEPROMCIESet : Set CIE EEPROM reference string - * - * - Parameters : - * int SocketIndex - * int CardNumber - * char *ReferenceString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEEPROMCIESet (int SocketIndex, int CardNumber, char * ReferenceString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EEPROMCIESet (%d,%s)", CardNumber, ReferenceString); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EEPROMDACOffsetCIESet : Set CIE DAC offsets - * - * - Parameters : - * int SocketIndex - * int PlugNumber - * double DAC1Offset - * double DAC2Offset - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEEPROMDACOffsetCIESet (int SocketIndex, int PlugNumber, double DAC1Offset, double DAC2Offset) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EEPROMDACOffsetCIESet (%d,%.13g,%.13g)", PlugNumber, DAC1Offset, DAC2Offset); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EEPROMDriverSet : Set Driver EEPROM reference string - * - * - Parameters : - * int SocketIndex - * int PlugNumber - * char *ReferenceString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEEPROMDriverSet (int SocketIndex, int PlugNumber, char * ReferenceString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EEPROMDriverSet (%d,%s)", PlugNumber, ReferenceString); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EEPROMINTSet : Set INT EEPROM reference string - * - * - Parameters : - * int SocketIndex - * int CardNumber - * char *ReferenceString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEEPROMINTSet (int SocketIndex, int CardNumber, char * ReferenceString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "EEPROMINTSet (%d,%s)", CardNumber, ReferenceString); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * CPUCoreAndBoardSupplyVoltagesGet : Get power informations - * - * - Parameters : - * int SocketIndex - * double *VoltageCPUCore - * double *SupplyVoltage1P5V - * double *SupplyVoltage3P3V - * double *SupplyVoltage5V - * double *SupplyVoltage12V - * double *SupplyVoltageM12V - * double *SupplyVoltageM5V - * double *SupplyVoltage5VSB - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPCPUCoreAndBoardSupplyVoltagesGet (int SocketIndex, double * VoltageCPUCore, double * SupplyVoltage1P5V, double * SupplyVoltage3P3V, double * SupplyVoltage5V, double * SupplyVoltage12V, double * SupplyVoltageM12V, double * SupplyVoltageM5V, double * SupplyVoltage5VSB) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "CPUCoreAndBoardSupplyVoltagesGet (double *,double *,double *,double *,double *,double *,double *,double *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", VoltageCPUCore); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage1P5V); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage3P3V); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage5V); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage12V); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SupplyVoltageM12V); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SupplyVoltageM5V); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage5VSB); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * CPUTemperatureAndFanSpeedGet : Get CPU temperature and fan speed - * - * - Parameters : - * int SocketIndex - * double *CPUTemperature - * double *CPUFanSpeed - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPCPUTemperatureAndFanSpeedGet (int SocketIndex, double * CPUTemperature, double * CPUFanSpeed) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "CPUTemperatureAndFanSpeedGet (double *,double *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CPUTemperature); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", CPUFanSpeed); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ActionListGet : Action list - * - * - Parameters : - * int SocketIndex - * char *ActionList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPActionListGet (int SocketIndex, char * ActionList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "ActionListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ActionList, pt); - ptNext = strchr (ActionList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ActionExtendedListGet : Action extended list - * - * - Parameters : - * int SocketIndex - * char *ActionList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPActionExtendedListGet (int SocketIndex, char * ActionList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "ActionExtendedListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ActionList, pt); - ptNext = strchr (ActionList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * APIExtendedListGet : API method list - * - * - Parameters : - * int SocketIndex - * char *Method - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPAPIExtendedListGet (int SocketIndex, char * Method) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); - - /* Convert to string */ - sprintf (ExecuteMethod, "APIExtendedListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (Method, pt); - ptNext = strchr (Method, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * APIListGet : API method list without extended API - * - * - Parameters : - * int SocketIndex - * char *Method - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPAPIListGet (int SocketIndex, char * Method) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); - - /* Convert to string */ - sprintf (ExecuteMethod, "APIListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (Method, pt); - ptNext = strchr (Method, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ErrorListGet : Error list - * - * - Parameters : - * int SocketIndex - * char *ErrorsList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPErrorListGet (int SocketIndex, char * ErrorsList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); - - /* Convert to string */ - sprintf (ExecuteMethod, "ErrorListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ErrorsList, pt); - ptNext = strchr (ErrorsList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * EventListGet : General event list - * - * - Parameters : - * int SocketIndex - * char *EventList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPEventListGet (int SocketIndex, char * EventList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "EventListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (EventList, pt); - ptNext = strchr (EventList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringListGet : Gathering type list - * - * - Parameters : - * int SocketIndex - * char *list - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringListGet (int SocketIndex, char * list) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (list, pt); - ptNext = strchr (list, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringExtendedListGet : Gathering type extended list - * - * - Parameters : - * int SocketIndex - * char *list - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringExtendedListGet (int SocketIndex, char * list) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringExtendedListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (list, pt); - ptNext = strchr (list, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringExternalListGet : External Gathering type list - * - * - Parameters : - * int SocketIndex - * char *list - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringExternalListGet (int SocketIndex, char * list) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringExternalListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (list, pt); - ptNext = strchr (list, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GroupStatusListGet : Group status list - * - * - Parameters : - * int SocketIndex - * char *GroupStatusList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGroupStatusListGet (int SocketIndex, char * GroupStatusList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); - - /* Convert to string */ - sprintf (ExecuteMethod, "GroupStatusListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (GroupStatusList, pt); - ptNext = strchr (GroupStatusList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * HardwareInternalListGet : Internal hardware list - * - * - Parameters : - * int SocketIndex - * char *InternalHardwareList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPHardwareInternalListGet (int SocketIndex, char * InternalHardwareList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); - - /* Convert to string */ - sprintf (ExecuteMethod, "HardwareInternalListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (InternalHardwareList, pt); - ptNext = strchr (InternalHardwareList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * HardwareDriverAndStageGet : Smart hardware - * - * - Parameters : - * int SocketIndex - * int PlugNumber - * char *DriverName - * char *StageName - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPHardwareDriverAndStageGet (int SocketIndex, int PlugNumber, char * DriverName, char * StageName) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); - - /* Convert to string */ - sprintf (ExecuteMethod, "HardwareDriverAndStageGet (%d,char *,char *)", PlugNumber); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (DriverName, pt); - ptNext = strchr (DriverName, ','); - if (ptNext != NULL) *ptNext = '\0'; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (StageName, pt); - ptNext = strchr (StageName, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ObjectsListGet : Group name and positioner name - * - * - Parameters : - * int SocketIndex - * char *ObjectsList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPObjectsListGet (int SocketIndex, char * ObjectsList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); - - /* Convert to string */ - sprintf (ExecuteMethod, "ObjectsListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ObjectsList, pt); - ptNext = strchr (ObjectsList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerErrorListGet : Positioner error list - * - * - Parameters : - * int SocketIndex - * char *PositionerErrorList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerErrorListGet (int SocketIndex, char * PositionerErrorList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerErrorListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (PositionerErrorList, pt); - ptNext = strchr (PositionerErrorList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerHardwareStatusListGet : Positioner hardware status list - * - * - Parameters : - * int SocketIndex - * char *PositionerHardwareStatusList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerHardwareStatusListGet (int SocketIndex, char * PositionerHardwareStatusList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerHardwareStatusListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (PositionerHardwareStatusList, pt); - ptNext = strchr (PositionerHardwareStatusList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PositionerDriverStatusListGet : Positioner driver status list - * - * - Parameters : - * int SocketIndex - * char *PositionerDriverStatusList - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPositionerDriverStatusListGet (int SocketIndex, char * PositionerDriverStatusList) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "PositionerDriverStatusListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (PositionerDriverStatusList, pt); - ptNext = strchr (PositionerDriverStatusList, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ReferencingActionListGet : Get referencing action list - * - * - Parameters : - * int SocketIndex - * char *list - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPReferencingActionListGet (int SocketIndex, char * list) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "ReferencingActionListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (list, pt); - ptNext = strchr (list, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ReferencingSensorListGet : Get referencing sensor list - * - * - Parameters : - * int SocketIndex - * char *list - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPReferencingSensorListGet (int SocketIndex, char * list) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); - - /* Convert to string */ - sprintf (ExecuteMethod, "ReferencingSensorListGet (char *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (list, pt); - ptNext = strchr (list, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * GatheringUserDatasGet : Return UserDatas values - * - * - Parameters : - * int SocketIndex - * double *UserData1 - * double *UserData2 - * double *UserData3 - * double *UserData4 - * double *UserData5 - * double *UserData6 - * double *UserData7 - * double *UserData8 - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPGatheringUserDatasGet (int SocketIndex, double * UserData1, double * UserData2, double * UserData3, double * UserData4, double * UserData5, double * UserData6, double * UserData7, double * UserData8) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "GatheringUserDatasGet (double *,double *,double *,double *,double *,double *,double *,double *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserData1); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserData2); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserData3); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserData4); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserData5); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserData6); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserData7); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", UserData8); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ControllerMotionKernelPeriodMinMaxGet : Get controller motion kernel min/max periods - * - * - Parameters : - * int SocketIndex - * double *MinimumCorrectorPeriod - * double *MaximumCorrectorPeriod - * double *MinimumProfilerPeriod - * double *MaximumProfilerPeriod - * double *MinimumServitudesPeriod - * double *MaximumServitudesPeriod - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPControllerMotionKernelPeriodMinMaxGet (int SocketIndex, double * MinimumCorrectorPeriod, double * MaximumCorrectorPeriod, double * MinimumProfilerPeriod, double * MaximumProfilerPeriod, double * MinimumServitudesPeriod, double * MaximumServitudesPeriod) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "ControllerMotionKernelPeriodMinMaxGet (double *,double *,double *,double *,double *,double *)"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MinimumCorrectorPeriod); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MaximumCorrectorPeriod); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MinimumProfilerPeriod); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MaximumProfilerPeriod); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MinimumServitudesPeriod); - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) sscanf (pt, "%lf", MaximumServitudesPeriod); - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * ControllerMotionKernelPeriodMinMaxReset : Reset controller motion kernel min/max periods - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPControllerMotionKernelPeriodMinMaxReset (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "ControllerMotionKernelPeriodMinMaxReset ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * TestTCP : Test TCP/IP transfert - * - * - Parameters : - * int SocketIndex - * char *InputString - * char *ReturnString - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPTestTCP (int SocketIndex, char * InputString, char * ReturnString) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "TestTCP (%s,char *)", InputString); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (ret == 0) - { - char * pt; - char * ptNext; - - pt = ReturnedValue; - ptNext = NULL; - if (pt != NULL) pt = strchr (pt, ','); - if (pt != NULL) pt++; - if (pt != NULL) strcpy (ReturnString, pt); - ptNext = strchr (ReturnString, ','); - if (ptNext != NULL) *ptNext = '\0'; - } - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - -/*********************************************************************** - * PrepareForUpdate : Kill QNX processes for firmware update - * - * - Parameters : - * int SocketIndex - * - Return : - * int errorCode - ***********************************************************************/ -int __stdcall HXPPrepareForUpdate (int SocketIndex) -{ - int ret = -1; - char ExecuteMethod[SIZE_EXECUTE_METHOD]; - char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); - - /* Convert to string */ - sprintf (ExecuteMethod, "PrepareForUpdate ()"); - - /* Send this string and wait return function from controller */ - /* return function : ==0 -> OK ; < 0 -> NOK */ - SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); - if (strlen (ReturnedValue) > 0) - sscanf (ReturnedValue, "%i", &ret); - - /* Get the returned values in the out parameters */ - if (NULL != ReturnedValue) - free (ReturnedValue); - - return (ret); -} - - - -#ifdef __cplusplus -} -#endif +/* + * Created source file hxp_drivers.cpp for API description + */ + + +#include +#include +#include +#include +#include "Socket.h" + +#ifdef _WIN32 + #define DLL _declspec(dllexport) + #include "strtok_r.h" +#else + #define DLL +#endif + +#include "hxp_drivers.h" + + + +#define SIZE_SMALL 1024 +#define SIZE_NOMINAL 1024 +#define SIZE_BIG 2048 +#define SIZE_HUGE 65536 + +#define SIZE_EXECUTE_METHOD 1024 + +#define SIZE_NAME 100 +#ifdef __cplusplus +extern "C" +{ +#else +#typedef int bool; /* C does not know bool, only C++ */ +#endif + + + +#define DLL_VERSION "Library version for HXP Firmware V1.3.x" + +/************************************************************************* +* Replace 'oldChar' by 'newChar' only between the startChar and endChar +*************************************************************************/ +void ReplaceCharacter (char *strSourceInOut, char oldChar, char newChar, char startChar, char endChar) +{ + char *pt; + char *ptNext; + ptNext = strSourceInOut; + do + { + pt = strchr(ptNext, startChar); + if (pt != NULL) + { + *pt++; + while ((pt != NULL) && (*pt != endChar)) + { + if (*pt == oldChar) + *pt = newChar; + pt++; + } + ptNext = pt++; + } + } + while ((pt != NULL) && (ptNext != NULL)); +} + +/************************************************************************* +* Delete space and tabulation characters between 'startChar' and 'endChar' +*************************************************************************/ +void CleanString (char *strSourceInOut, char startChar, char endChar) +{ + int len = 0; + int startIndex = 0; + int endIndex = 0; + int outputIndex = 0; + char outputString[SIZE_NOMINAL]; + len = strlen(strSourceInOut); + do + { + while ((strSourceInOut[startIndex] != startChar) && (startIndex < len) && (outputIndex < SIZE_NOMINAL)) + { + outputString[outputIndex] = strSourceInOut[startIndex]; + outputIndex++; + startIndex++; + } + while ((strSourceInOut[endIndex] != endChar) && (endIndex < len)) + endIndex++; + if ((startIndex != endIndex) && (startIndex < len)) + { + for (int i = startIndex; (i <= endIndex) && (outputIndex < SIZE_NOMINAL); i++) + { + if ((strSourceInOut[i] != ' ') && (strSourceInOut[i] != '\t')) + { + outputString[outputIndex] = strSourceInOut[i]; + outputIndex++; + } + } + endIndex++; + startIndex = endIndex; + } + } + while (startIndex < len); + outputString[outputIndex] = '\0'; + strcpy (strSourceInOut, outputString); +} + +/************************************************************************* +* Delete a specified characters +*************************************************************************/ +void DeleteCharacters (char *strSourceInOut, char *charactersToDelete) +{ + int len = 0; + int nbChar = 0; + int outputIndex = 0; + bool bCopy; + char outputString[SIZE_NOMINAL]; + len = strlen(strSourceInOut); + nbChar = strlen(charactersToDelete); + for (int i = 0; (i <= len) && (outputIndex < SIZE_NOMINAL); i++) + { + bCopy = true; + for (int j = 0; (j < nbChar) && (bCopy == true); j++) + { + if (strSourceInOut[i] == charactersToDelete[j]) + bCopy = false; + } + if (bCopy) + { + outputString[outputIndex] = strSourceInOut[i]; + outputIndex++; + } + } + outputString[outputIndex] = '\0'; + strcpy (strSourceInOut, outputString); +} + +/***********************************************************************/ +int __stdcall HXPTCP_ConnectToServer(char *Ip_Address, int Ip_Port, double TimeOut) +{ + return (ConnectToServer(Ip_Address, Ip_Port, TimeOut)); +} +/***********************************************************************/ +void __stdcall HXPTCP_SetTimeout(int SocketIndex, double Timeout) +{ + SetTCPTimeout(SocketIndex, Timeout); +} +/***********************************************************************/ +void __stdcall HXPTCP_CloseSocket(int SocketIndex) +{ + CloseSocket(SocketIndex); +} +/***********************************************************************/ +char * __stdcall HXPTCP_GetError(int SocketIndex) +{ + return (GetError(SocketIndex)); +} +/***********************************************************************/ +char * __stdcall HXPGetLibraryVersion(void) +{ + return (DLL_VERSION); +} + +/*********************************************************************** + * ControllerMotionKernelTimeLoadGet : Get controller motion kernel time load + * + * - Parameters : + * int SocketIndex + * double *CPUTotalLoadRatio + * double *CPUCorrectorLoadRatio + * double *CPUProfilerLoadRatio + * double *CPUServitudesLoadRatio + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPControllerMotionKernelTimeLoadGet (int SocketIndex, double * CPUTotalLoadRatio, double * CPUCorrectorLoadRatio, double * CPUProfilerLoadRatio, double * CPUServitudesLoadRatio) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "ControllerMotionKernelTimeLoadGet (double *,double *,double *,double *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CPUTotalLoadRatio); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CPUCorrectorLoadRatio); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CPUProfilerLoadRatio); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CPUServitudesLoadRatio); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ElapsedTimeGet : Return elapsed time from controller power on + * + * - Parameters : + * int SocketIndex + * double *ElapsedTime + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPElapsedTimeGet (int SocketIndex, double * ElapsedTime) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "ElapsedTimeGet (double *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", ElapsedTime); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ErrorStringGet : Return the error string corresponding to the error code + * + * - Parameters : + * int SocketIndex + * int ErrorCode + * char *ErrorString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPErrorStringGet (int SocketIndex, int ErrorCode, char * ErrorString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "ErrorStringGet (%d,char *)", ErrorCode); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ErrorString, pt); + ptNext = strchr (ErrorString, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * FirmwareVersionGet : Return firmware version + * + * - Parameters : + * int SocketIndex + * char *Version + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPFirmwareVersionGet (int SocketIndex, char * Version) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "FirmwareVersionGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (Version, pt); + ptNext = strchr (Version, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * TCLScriptExecute : Execute a TCL script from a TCL file + * + * - Parameters : + * int SocketIndex + * char *TCLFileName + * char *TaskName + * char *ParametersList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPTCLScriptExecute (int SocketIndex, char * TCLFileName, char * TaskName, char * ParametersList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "TCLScriptExecute (%s,%s,%s)", TCLFileName, TaskName, ParametersList); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * TCLScriptExecuteAndWait : Execute a TCL script from a TCL file and wait the end of execution to return + * + * - Parameters : + * int SocketIndex + * char *TCLFileName + * char *TaskName + * char *InputParametersList + * char *OutputParametersList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPTCLScriptExecuteAndWait (int SocketIndex, char * TCLFileName, char * TaskName, char * InputParametersList, char * OutputParametersList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "TCLScriptExecuteAndWait (%s,%s,%s,char *)", TCLFileName, TaskName, InputParametersList); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (OutputParametersList, pt); + ptNext = strchr (OutputParametersList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * TCLScriptKill : Kill TCL Task + * + * - Parameters : + * int SocketIndex + * char *TaskName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPTCLScriptKill (int SocketIndex, char * TaskName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "TCLScriptKill (%s)", TaskName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * TimerGet : Get a timer + * + * - Parameters : + * int SocketIndex + * char *TimerName + * int *FrequencyTicks + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPTimerGet (int SocketIndex, char * TimerName, int * FrequencyTicks) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "TimerGet (%s,int *)", TimerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", FrequencyTicks); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * TimerSet : Set a timer + * + * - Parameters : + * int SocketIndex + * char *TimerName + * int FrequencyTicks + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPTimerSet (int SocketIndex, char * TimerName, int FrequencyTicks) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "TimerSet (%s,%d)", TimerName, FrequencyTicks); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * Reboot : Reboot the controller + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPReboot (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "Reboot ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * Login : Log in + * + * - Parameters : + * int SocketIndex + * char *Name + * char *Password + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPLogin (int SocketIndex, char * Name, char * Password) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "Login (%s,%s)", Name, Password); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * CloseAllOtherSockets : Close all socket beside the one used to send this command + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPCloseAllOtherSockets (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "CloseAllOtherSockets ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventAdd : ** OBSOLETE ** Add an event + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * char *EventName + * char *EventParameter + * char *ActionName + * char *ActionParameter1 + * char *ActionParameter2 + * char *ActionParameter3 + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventAdd (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter, char * ActionName, char * ActionParameter1, char * ActionParameter2, char * ActionParameter3) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventAdd (%s,%s,%s,%s,%s,%s,%s)", PositionerName, EventName, EventParameter, ActionName, ActionParameter1, ActionParameter2, ActionParameter3); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventGet : ** OBSOLETE ** Read events and actions list + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * char *EventsAndActionsList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventGet (int SocketIndex, char * PositionerName, char * EventsAndActionsList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventGet (%s,char *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (EventsAndActionsList, pt); + ptNext = strchr (EventsAndActionsList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventRemove : ** OBSOLETE ** Delete an event + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * char *EventName + * char *EventParameter + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventRemove (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventRemove (%s,%s,%s)", PositionerName, EventName, EventParameter); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventWait : ** OBSOLETE ** Wait an event + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * char *EventName + * char *EventParameter + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventWait (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventWait (%s,%s,%s)", PositionerName, EventName, EventParameter); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedConfigurationTriggerSet : Configure one or several events + * + * - Parameters : + * int SocketIndex + * int nbElement + * char *ExtendedEventName + * char *EventParameter1 + * char *EventParameter2 + * char *EventParameter3 + * char *EventParameter4 + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedConfigurationTriggerSet (int SocketIndex, int NbElements, char * ExtendedEventNameList, char * EventParameter1List, char * EventParameter2List, char * EventParameter3List, char * EventParameter4List) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Split list */ + char *token; + char seps[] = " \t;"; + int indice; + char list [SIZE_NOMINAL]; + char *list_r; + char subString[] = "{}"; + + char (*stringArray0)[SIZE_NAME]; + stringArray0 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, ExtendedEventNameList, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray0[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray0[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + char (*stringArray1)[SIZE_NAME]; + stringArray1 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, EventParameter1List, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray1[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray1[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray1[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray1[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + char (*stringArray2)[SIZE_NAME]; + stringArray2 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, EventParameter2List, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray2[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray2[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray2[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray2[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + char (*stringArray3)[SIZE_NAME]; + stringArray3 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, EventParameter3List, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray3[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray3[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray3[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray3[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + char (*stringArray4)[SIZE_NAME]; + stringArray4 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, EventParameter4List, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray4[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray4[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray4[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray4[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedConfigurationTriggerSet ("); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%s,%s,%s,%s,%s", stringArray0[i], stringArray1[i], stringArray2[i], stringArray3[i], stringArray4[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Clear memory */ + delete [] stringArray0; + delete [] stringArray1; + delete [] stringArray2; + delete [] stringArray3; + delete [] stringArray4; + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedConfigurationTriggerGet : Read the event configuration + * + * - Parameters : + * int SocketIndex + * char *EventTriggerConfiguration + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedConfigurationTriggerGet (int SocketIndex, char * EventTriggerConfiguration) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedConfigurationTriggerGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (EventTriggerConfiguration, pt); + ptNext = strchr (EventTriggerConfiguration, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedConfigurationActionSet : Configure one or several actions + * + * - Parameters : + * int SocketIndex + * int nbElement + * char *ExtendedActionName + * char *ActionParameter1 + * char *ActionParameter2 + * char *ActionParameter3 + * char *ActionParameter4 + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedConfigurationActionSet (int SocketIndex, int NbElements, char * ExtendedActionNameList, char * ActionParameter1List, char * ActionParameter2List, char * ActionParameter3List, char * ActionParameter4List) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Split list */ + char *token; + char seps[] = " \t;"; + int indice; + char list [SIZE_NOMINAL]; + char *list_r; + char subString[] = "{}"; + + char (*stringArray0)[SIZE_NAME]; + stringArray0 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, ExtendedActionNameList, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray0[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray0[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + char (*stringArray1)[SIZE_NAME]; + stringArray1 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, ActionParameter1List, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray1[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray1[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray1[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray1[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + char (*stringArray2)[SIZE_NAME]; + stringArray2 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, ActionParameter2List, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray2[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray2[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray2[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray2[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + char (*stringArray3)[SIZE_NAME]; + stringArray3 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, ActionParameter3List, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray3[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray3[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray3[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray3[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + char (*stringArray4)[SIZE_NAME]; + stringArray4 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, ActionParameter4List, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray4[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray4[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray4[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray4[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedConfigurationActionSet ("); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%s,%s,%s,%s,%s", stringArray0[i], stringArray1[i], stringArray2[i], stringArray3[i], stringArray4[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Clear memory */ + delete [] stringArray0; + delete [] stringArray1; + delete [] stringArray2; + delete [] stringArray3; + delete [] stringArray4; + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedConfigurationActionGet : Read the action configuration + * + * - Parameters : + * int SocketIndex + * char *ActionConfiguration + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedConfigurationActionGet (int SocketIndex, char * ActionConfiguration) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedConfigurationActionGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ActionConfiguration, pt); + ptNext = strchr (ActionConfiguration, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedStart : Launch the last event and action configuration and return an ID + * + * - Parameters : + * int SocketIndex + * int *ID + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedStart (int SocketIndex, int * ID) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedStart (int *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", ID); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedAllGet : Read all event and action configurations + * + * - Parameters : + * int SocketIndex + * char *EventActionConfigurations + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedAllGet (int SocketIndex, char * EventActionConfigurations) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedAllGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (EventActionConfigurations, pt); + ptNext = strchr (EventActionConfigurations, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedGet : Read the event and action configuration defined by ID + * + * - Parameters : + * int SocketIndex + * int ID + * char *EventTriggerConfiguration + * char *ActionConfiguration + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedGet (int SocketIndex, int ID, char * EventTriggerConfiguration, char * ActionConfiguration) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedGet (%d,char *,char *)", ID); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (EventTriggerConfiguration, pt); + ptNext = strchr (EventTriggerConfiguration, ','); + if (ptNext != NULL) *ptNext = '\0'; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ActionConfiguration, pt); + ptNext = strchr (ActionConfiguration, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedRemove : Remove the event and action configuration defined by ID + * + * - Parameters : + * int SocketIndex + * int ID + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedRemove (int SocketIndex, int ID) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedRemove (%d)", ID); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventExtendedWait : Wait events from the last event configuration + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventExtendedWait (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventExtendedWait ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringConfigurationGet : Read different mnemonique type + * + * - Parameters : + * int SocketIndex + * char *Type + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringConfigurationGet (int SocketIndex, char * Type) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringConfigurationGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (Type, pt); + ptNext = strchr (Type, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringConfigurationSet : Configuration acquisition + * + * - Parameters : + * int SocketIndex + * int nbElement + * char *Type + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringConfigurationSet (int SocketIndex, int NbElements, char * TypeList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Split list */ + char *token; + char seps[] = " \t;"; + int indice; + char list [SIZE_NOMINAL]; + char *list_r; + char subString[] = "{}"; + + char (*stringArray0)[SIZE_NAME]; + stringArray0 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, TypeList, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray0[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray0[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringConfigurationSet ("); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%s", stringArray0[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Clear memory */ + delete [] stringArray0; + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringCurrentNumberGet : Maximum number of samples and current number during acquisition + * + * - Parameters : + * int SocketIndex + * int *CurrentNumber + * int *MaximumSamplesNumber + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringCurrentNumberGet (int SocketIndex, int * CurrentNumber, int * MaximumSamplesNumber) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringCurrentNumberGet (int *,int *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", CurrentNumber); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", MaximumSamplesNumber); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringStopAndSave : Stop acquisition and save data + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringStopAndSave (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringStopAndSave ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringDataAcquire : Acquire a configured data + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringDataAcquire (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringDataAcquire ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringDataGet : Get a data line from gathering buffer + * + * - Parameters : + * int SocketIndex + * int IndexPoint + * char *DataBufferLine + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringDataGet (int SocketIndex, int IndexPoint, char * DataBufferLine) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringDataGet (%d,char *)", IndexPoint); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (DataBufferLine, pt); + ptNext = strchr (DataBufferLine, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringReset : Empty the gathered data in memory to start new gathering from scratch + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringReset (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringReset ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringRun : Start a new gathering + * + * - Parameters : + * int SocketIndex + * int DataNumber + * int Divisor + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringRun (int SocketIndex, int DataNumber, int Divisor) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringRun (%d,%d)", DataNumber, Divisor); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringStop : Stop the data gathering (without saving to file) + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringStop (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringStop ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringExternalConfigurationSet : Configuration acquisition + * + * - Parameters : + * int SocketIndex + * int nbElement + * char *Type + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringExternalConfigurationSet (int SocketIndex, int NbElements, char * TypeList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Split list */ + char *token; + char seps[] = " \t;"; + int indice; + char list [SIZE_NOMINAL]; + char *list_r; + char subString[] = "{}"; + + char (*stringArray0)[SIZE_NAME]; + stringArray0 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, TypeList, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray0[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray0[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringExternalConfigurationSet ("); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%s", stringArray0[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Clear memory */ + delete [] stringArray0; + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringExternalConfigurationGet : Read different mnemonique type + * + * - Parameters : + * int SocketIndex + * char *Type + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringExternalConfigurationGet (int SocketIndex, char * Type) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringExternalConfigurationGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (Type, pt); + ptNext = strchr (Type, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringExternalCurrentNumberGet : Maximum number of samples and current number during acquisition + * + * - Parameters : + * int SocketIndex + * int *CurrentNumber + * int *MaximumSamplesNumber + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringExternalCurrentNumberGet (int SocketIndex, int * CurrentNumber, int * MaximumSamplesNumber) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringExternalCurrentNumberGet (int *,int *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", CurrentNumber); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", MaximumSamplesNumber); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringExternalStopAndSave : Stop acquisition and save data + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringExternalStopAndSave (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringExternalStopAndSave ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GlobalArrayGet : Get global array value + * + * - Parameters : + * int SocketIndex + * int Number + * char *ValueString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGlobalArrayGet (int SocketIndex, int Number, char * ValueString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GlobalArrayGet (%d,char *)", Number); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ValueString, pt); + ptNext = strchr (ValueString, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GlobalArraySet : Set global array value + * + * - Parameters : + * int SocketIndex + * int Number + * char *ValueString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGlobalArraySet (int SocketIndex, int Number, char * ValueString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GlobalArraySet (%d,%s)", Number, ValueString); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * DoubleGlobalArrayGet : Get double global array value + * + * - Parameters : + * int SocketIndex + * int Number + * double *DoubleValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPDoubleGlobalArrayGet (int SocketIndex, int Number, double * DoubleValue) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "DoubleGlobalArrayGet (%d,double *)", Number); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", DoubleValue); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * DoubleGlobalArraySet : Set double global array value + * + * - Parameters : + * int SocketIndex + * int Number + * double DoubleValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPDoubleGlobalArraySet (int SocketIndex, int Number, double DoubleValue) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "DoubleGlobalArraySet (%d,%.13g)", Number, DoubleValue); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GPIOAnalogGet : Read analog input or analog output for one or few input + * + * - Parameters : + * int SocketIndex + * int nbElement + * char *GPIOName + * double *AnalogValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGPIOAnalogGet (int SocketIndex, int NbElements, char * GPIONameList, double AnalogValue[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Split list */ + char *token; + char seps[] = " \t;"; + int indice; + char list [SIZE_NOMINAL]; + char *list_r; + char subString[] = "{}"; + + char (*stringArray0)[SIZE_NAME]; + stringArray0 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, GPIONameList, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray0[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray0[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + + /* Convert to string */ + sprintf (ExecuteMethod, "GPIOAnalogGet ("); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%s,double *", stringArray0[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Clear memory */ + delete [] stringArray0; + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + + for (int i = 0; i < NbElements; i++) + { + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", &AnalogValue[i]); + } + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GPIOAnalogSet : Set analog output for one or few output + * + * - Parameters : + * int SocketIndex + * int nbElement + * char *GPIOName + * double AnalogOutputValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGPIOAnalogSet (int SocketIndex, int NbElements, char * GPIONameList, double AnalogOutputValue[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Split list */ + char *token; + char seps[] = " \t;"; + int indice; + char list [SIZE_NOMINAL]; + char *list_r; + char subString[] = "{}"; + + char (*stringArray0)[SIZE_NAME]; + stringArray0 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, GPIONameList, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray0[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray0[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + + /* Convert to string */ + sprintf (ExecuteMethod, "GPIOAnalogSet ("); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%s,%.13g", stringArray0[i], AnalogOutputValue[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Clear memory */ + delete [] stringArray0; + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GPIOAnalogGainGet : Read analog input gain (1, 2, 4 or 8) for one or few input + * + * - Parameters : + * int SocketIndex + * int nbElement + * char *GPIOName + * int *AnalogInputGainValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGPIOAnalogGainGet (int SocketIndex, int NbElements, char * GPIONameList, int AnalogInputGainValue[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Split list */ + char *token; + char seps[] = " \t;"; + int indice; + char list [SIZE_NOMINAL]; + char *list_r; + char subString[] = "{}"; + + char (*stringArray0)[SIZE_NAME]; + stringArray0 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, GPIONameList, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray0[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray0[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + + /* Convert to string */ + sprintf (ExecuteMethod, "GPIOAnalogGainGet ("); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%s,int *", stringArray0[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Clear memory */ + delete [] stringArray0; + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + + for (int i = 0; i < NbElements; i++) + { + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", &AnalogInputGainValue[i]); + } + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GPIOAnalogGainSet : Set analog input gain (1, 2, 4 or 8) for one or few input + * + * - Parameters : + * int SocketIndex + * int nbElement + * char *GPIOName + * int AnalogInputGainValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGPIOAnalogGainSet (int SocketIndex, int NbElements, char * GPIONameList, int AnalogInputGainValue[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Split list */ + char *token; + char seps[] = " \t;"; + int indice; + char list [SIZE_NOMINAL]; + char *list_r; + char subString[] = "{}"; + + char (*stringArray0)[SIZE_NAME]; + stringArray0 = new char [NbElements][SIZE_NAME]; + indice = 0; + strncpyWithEOS(list, GPIONameList, SIZE_NOMINAL, SIZE_NOMINAL); + ReplaceCharacter(list, ';', ':', '{', '}'); /* for argument {x1;x2} */ + CleanString(list, '{', '}'); + list_r = NULL; + token = strtok_r (list, seps, &list_r); + while ((NULL != token) && (indice < NbElements)) + { + memset(stringArray0[indice],'\0', SIZE_NAME); + strncpyWithEOS(stringArray0[indice], token, SIZE_NAME, SIZE_NAME); + ReplaceCharacter (stringArray0[indice], ':', ';', '{', '}'); + DeleteCharacters (stringArray0[indice], subString); + token = strtok_r (NULL, seps, &list_r); + indice++; + } + + /* Convert to string */ + sprintf (ExecuteMethod, "GPIOAnalogGainSet ("); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%s,%d", stringArray0[i], AnalogInputGainValue[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Clear memory */ + delete [] stringArray0; + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GPIODigitalGet : Read digital output or digital input + * + * - Parameters : + * int SocketIndex + * char *GPIOName + * unsigned short *DigitalValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGPIODigitalGet (int SocketIndex, char * GPIOName, unsigned short * DigitalValue) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GPIODigitalGet (%s,unsigned short *)", GPIOName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%hu", DigitalValue); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GPIODigitalSet : Set Digital Output for one or few output TTL + * + * - Parameters : + * int SocketIndex + * char *GPIOName + * unsigned short Mask + * unsigned short DigitalOutputValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGPIODigitalSet (int SocketIndex, char * GPIOName, unsigned short Mask, unsigned short DigitalOutputValue) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GPIODigitalSet (%s,%hu,%hu)", GPIOName, Mask, DigitalOutputValue); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupCorrectorOutputGet : Return corrector outputs + * + * - Parameters : + * int SocketIndex + * char *GroupName + * int nbElement + * double *CorrectorOutput + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupCorrectorOutputGet (int SocketIndex, char * GroupName, int NbElements, double CorrectorOutput[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupCorrectorOutputGet (%s,", GroupName); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "double *"); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + + for (int i = 0; i < NbElements; i++) + { + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", &CorrectorOutput[i]); + } + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupHomeSearch : Start home search sequence + * + * - Parameters : + * int SocketIndex + * char *GroupName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupHomeSearch (int SocketIndex, char * GroupName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupHomeSearch (%s)", GroupName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupHomeSearchAndRelativeMove : Start home search sequence and execute a displacement + * + * - Parameters : + * int SocketIndex + * char *GroupName + * double TargetDisplacement + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupHomeSearchAndRelativeMove (int SocketIndex, char * GroupName, int NbElements, double TargetDisplacement[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupHomeSearchAndRelativeMove (%s,", GroupName); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%.13g", TargetDisplacement[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupInitialize : Start the initialization + * + * - Parameters : + * int SocketIndex + * char *GroupName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupInitialize (int SocketIndex, char * GroupName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupInitialize (%s)", GroupName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupInitializeWithEncoderCalibration : Start the initialization with encoder calibration + * + * - Parameters : + * int SocketIndex + * char *GroupName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupInitializeWithEncoderCalibration (int SocketIndex, char * GroupName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupInitializeWithEncoderCalibration (%s)", GroupName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupKill : Kill the group + * + * - Parameters : + * int SocketIndex + * char *GroupName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupKill (int SocketIndex, char * GroupName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupKill (%s)", GroupName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupMoveAbort : Abort a move + * + * - Parameters : + * int SocketIndex + * char *GroupName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupMoveAbort (int SocketIndex, char * GroupName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupMoveAbort (%s)", GroupName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupMoveAbsolute : Do an absolute move + * + * - Parameters : + * int SocketIndex + * char *GroupName + * double TargetPosition + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupMoveAbsolute (int SocketIndex, char * GroupName, int NbElements, double TargetPosition[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupMoveAbsolute (%s,", GroupName); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%.13g", TargetPosition[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupMoveRelative : Do a relative move + * + * - Parameters : + * int SocketIndex + * char *GroupName + * double TargetDisplacement + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupMoveRelative (int SocketIndex, char * GroupName, int NbElements, double TargetDisplacement[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupMoveRelative (%s,", GroupName); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "%.13g", TargetDisplacement[i]); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupMotionDisable : Set Motion disable on selected group + * + * - Parameters : + * int SocketIndex + * char *GroupName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupMotionDisable (int SocketIndex, char * GroupName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupMotionDisable (%s)", GroupName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupMotionEnable : Set Motion enable on selected group + * + * - Parameters : + * int SocketIndex + * char *GroupName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupMotionEnable (int SocketIndex, char * GroupName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupMotionEnable (%s)", GroupName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupPositionCorrectedProfilerGet : Return corrected profiler positions + * + * - Parameters : + * int SocketIndex + * char *GroupName + * double PositionX + * double PositionY + * double *CorrectedProfilerPositionX + * double *CorrectedProfilerPositionY + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupPositionCorrectedProfilerGet (int SocketIndex, char * GroupName, double PositionX, double PositionY, double * CorrectedProfilerPositionX, double * CorrectedProfilerPositionY) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupPositionCorrectedProfilerGet (%s,%.13g,%.13g,double *,double *)", GroupName, PositionX, PositionY); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CorrectedProfilerPositionX); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CorrectedProfilerPositionY); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupPositionCurrentGet : Return current positions + * + * - Parameters : + * int SocketIndex + * char *GroupName + * int nbElement + * double *CurrentEncoderPosition + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupPositionCurrentGet (int SocketIndex, char * GroupName, int NbElements, double CurrentEncoderPosition[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupPositionCurrentGet (%s,", GroupName); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "double *"); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + + for (int i = 0; i < NbElements; i++) + { + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", &CurrentEncoderPosition[i]); + } + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupPositionSetpointGet : Return setpoint positions + * + * - Parameters : + * int SocketIndex + * char *GroupName + * int nbElement + * double *SetPointPosition + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupPositionSetpointGet (int SocketIndex, char * GroupName, int NbElements, double SetPointPosition[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupPositionSetpointGet (%s,", GroupName); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "double *"); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + + for (int i = 0; i < NbElements; i++) + { + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", &SetPointPosition[i]); + } + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupPositionTargetGet : Return target positions + * + * - Parameters : + * int SocketIndex + * char *GroupName + * int nbElement + * double *TargetPosition + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupPositionTargetGet (int SocketIndex, char * GroupName, int NbElements, double TargetPosition[]) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + char temp[SIZE_NOMINAL]; + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupPositionTargetGet (%s,", GroupName); + for (int i = 0; i < NbElements; i++) + { + sprintf (temp, "double *"); + strncat (ExecuteMethod, temp, SIZE_SMALL); + if ((i + 1) < NbElements) + { + strncat (ExecuteMethod, ",", SIZE_SMALL); + } + } + strcat (ExecuteMethod, ")"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + + for (int i = 0; i < NbElements; i++) + { + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", &TargetPosition[i]); + } + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupStatusGet : Return group status + * + * - Parameters : + * int SocketIndex + * char *GroupName + * int *Status + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupStatusGet (int SocketIndex, char * GroupName, int * Status) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupStatusGet (%s,int *)", GroupName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", Status); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupStatusStringGet : Return the group status string corresponding to the group status code + * + * - Parameters : + * int SocketIndex + * int GroupStatusCode + * char *GroupStatusString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupStatusStringGet (int SocketIndex, int GroupStatusCode, char * GroupStatusString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupStatusStringGet (%d,char *)", GroupStatusCode); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (GroupStatusString, pt); + ptNext = strchr (GroupStatusString, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * KillAll : Put all groups in 'Not initialized' state + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPKillAll (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "KillAll ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * RestartApplication : Restart the Controller + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPRestartApplication (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "RestartApplication ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerBacklashGet : Read backlash value and status + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *BacklashValue + * char *BacklaskStatus + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerBacklashGet (int SocketIndex, char * PositionerName, double * BacklashValue, char * BacklaskStatus) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerBacklashGet (%s,double *,char *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", BacklashValue); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (BacklaskStatus, pt); + ptNext = strchr (BacklaskStatus, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerBacklashSet : Set backlash value + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double BacklashValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerBacklashSet (int SocketIndex, char * PositionerName, double BacklashValue) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerBacklashSet (%s,%.13g)", PositionerName, BacklashValue); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerBacklashEnable : Enable the backlash + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerBacklashEnable (int SocketIndex, char * PositionerName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerBacklashEnable (%s)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerBacklashDisable : Disable the backlash + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerBacklashDisable (int SocketIndex, char * PositionerName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerBacklashDisable (%s)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorNotchFiltersSet : Update filters parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double NotchFrequency1 + * double NotchBandwith1 + * double NotchGain1 + * double NotchFrequency2 + * double NotchBandwith2 + * double NotchGain2 + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorNotchFiltersSet (int SocketIndex, char * PositionerName, double NotchFrequency1, double NotchBandwith1, double NotchGain1, double NotchFrequency2, double NotchBandwith2, double NotchGain2) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorNotchFiltersSet (%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, NotchFrequency1, NotchBandwith1, NotchGain1, NotchFrequency2, NotchBandwith2, NotchGain2); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorNotchFiltersGet : Read filters parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *NotchFrequency1 + * double *NotchBandwith1 + * double *NotchGain1 + * double *NotchFrequency2 + * double *NotchBandwith2 + * double *NotchGain2 + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorNotchFiltersGet (int SocketIndex, char * PositionerName, double * NotchFrequency1, double * NotchBandwith1, double * NotchGain1, double * NotchFrequency2, double * NotchBandwith2, double * NotchGain2) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorNotchFiltersGet (%s,double *,double *,double *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", NotchFrequency1); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", NotchBandwith1); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", NotchGain1); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", NotchFrequency2); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", NotchBandwith2); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", NotchGain2); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorPIDFFAccelerationSet : Update corrector parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * bool ClosedLoopStatus + * double KP + * double KI + * double KD + * double KS + * double IntegrationTime + * double DerivativeFilterCutOffFrequency + * double GKP + * double GKI + * double GKD + * double KForm + * double FeedForwardGainAcceleration + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorPIDFFAccelerationSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainAcceleration) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorPIDFFAccelerationSet (%s,%d,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, ClosedLoopStatus, KP, KI, KD, KS, IntegrationTime, DerivativeFilterCutOffFrequency, GKP, GKI, GKD, KForm, FeedForwardGainAcceleration); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorPIDFFAccelerationGet : Read corrector parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * bool *ClosedLoopStatus + * double *KP + * double *KI + * double *KD + * double *KS + * double *IntegrationTime + * double *DerivativeFilterCutOffFrequency + * double *GKP + * double *GKI + * double *GKD + * double *KForm + * double *FeedForwardGainAcceleration + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorPIDFFAccelerationGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainAcceleration) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + int boolScanTmp; + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorPIDFFAccelerationGet (%s,bool *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", &boolScanTmp); + *ClosedLoopStatus = (bool) boolScanTmp; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KP); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KI); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KD); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KS); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", IntegrationTime); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", DerivativeFilterCutOffFrequency); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKP); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKI); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKD); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KForm); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", FeedForwardGainAcceleration); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorPIDFFVelocitySet : Update corrector parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * bool ClosedLoopStatus + * double KP + * double KI + * double KD + * double KS + * double IntegrationTime + * double DerivativeFilterCutOffFrequency + * double GKP + * double GKI + * double GKD + * double KForm + * double FeedForwardGainVelocity + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorPIDFFVelocitySet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainVelocity) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorPIDFFVelocitySet (%s,%d,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, ClosedLoopStatus, KP, KI, KD, KS, IntegrationTime, DerivativeFilterCutOffFrequency, GKP, GKI, GKD, KForm, FeedForwardGainVelocity); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorPIDFFVelocityGet : Read corrector parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * bool *ClosedLoopStatus + * double *KP + * double *KI + * double *KD + * double *KS + * double *IntegrationTime + * double *DerivativeFilterCutOffFrequency + * double *GKP + * double *GKI + * double *GKD + * double *KForm + * double *FeedForwardGainVelocity + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorPIDFFVelocityGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainVelocity) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + int boolScanTmp; + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorPIDFFVelocityGet (%s,bool *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", &boolScanTmp); + *ClosedLoopStatus = (bool) boolScanTmp; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KP); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KI); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KD); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KS); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", IntegrationTime); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", DerivativeFilterCutOffFrequency); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKP); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKI); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKD); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KForm); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", FeedForwardGainVelocity); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorPIDDualFFVoltageSet : Update corrector parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * bool ClosedLoopStatus + * double KP + * double KI + * double KD + * double KS + * double IntegrationTime + * double DerivativeFilterCutOffFrequency + * double GKP + * double GKI + * double GKD + * double KForm + * double FeedForwardGainVelocity + * double FeedForwardGainAcceleration + * double Friction + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorPIDDualFFVoltageSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainVelocity, double FeedForwardGainAcceleration, double Friction) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorPIDDualFFVoltageSet (%s,%d,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, ClosedLoopStatus, KP, KI, KD, KS, IntegrationTime, DerivativeFilterCutOffFrequency, GKP, GKI, GKD, KForm, FeedForwardGainVelocity, FeedForwardGainAcceleration, Friction); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorPIDDualFFVoltageGet : Read corrector parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * bool *ClosedLoopStatus + * double *KP + * double *KI + * double *KD + * double *KS + * double *IntegrationTime + * double *DerivativeFilterCutOffFrequency + * double *GKP + * double *GKI + * double *GKD + * double *KForm + * double *FeedForwardGainVelocity + * double *FeedForwardGainAcceleration + * double *Friction + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorPIDDualFFVoltageGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainVelocity, double * FeedForwardGainAcceleration, double * Friction) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + int boolScanTmp; + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorPIDDualFFVoltageGet (%s,bool *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", &boolScanTmp); + *ClosedLoopStatus = (bool) boolScanTmp; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KP); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KI); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KD); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KS); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", IntegrationTime); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", DerivativeFilterCutOffFrequency); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKP); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKI); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", GKD); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KForm); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", FeedForwardGainVelocity); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", FeedForwardGainAcceleration); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Friction); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorPIPositionSet : Update corrector parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * bool ClosedLoopStatus + * double KP + * double KI + * double IntegrationTime + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorPIPositionSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double IntegrationTime) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorPIPositionSet (%s,%d,%.13g,%.13g,%.13g)", PositionerName, ClosedLoopStatus, KP, KI, IntegrationTime); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorPIPositionGet : Read corrector parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * bool *ClosedLoopStatus + * double *KP + * double *KI + * double *IntegrationTime + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorPIPositionGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * IntegrationTime) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + int boolScanTmp; + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorPIPositionGet (%s,bool *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", &boolScanTmp); + *ClosedLoopStatus = (bool) boolScanTmp; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KP); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", KI); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", IntegrationTime); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCorrectorTypeGet : Read corrector type + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * char *CorrectorType + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCorrectorTypeGet (int SocketIndex, char * PositionerName, char * CorrectorType) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCorrectorTypeGet (%s,char *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (CorrectorType, pt); + ptNext = strchr (CorrectorType, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCurrentVelocityAccelerationFiltersSet : Set current velocity and acceleration cut off frequencies + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double CurrentVelocityCutOffFrequency + * double CurrentAccelerationCutOffFrequency + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCurrentVelocityAccelerationFiltersSet (int SocketIndex, char * PositionerName, double CurrentVelocityCutOffFrequency, double CurrentAccelerationCutOffFrequency) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCurrentVelocityAccelerationFiltersSet (%s,%.13g,%.13g)", PositionerName, CurrentVelocityCutOffFrequency, CurrentAccelerationCutOffFrequency); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerCurrentVelocityAccelerationFiltersGet : Get current velocity and acceleration cut off frequencies + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *CurrentVelocityCutOffFrequency + * double *CurrentAccelerationCutOffFrequency + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerCurrentVelocityAccelerationFiltersGet (int SocketIndex, char * PositionerName, double * CurrentVelocityCutOffFrequency, double * CurrentAccelerationCutOffFrequency) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerCurrentVelocityAccelerationFiltersGet (%s,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CurrentVelocityCutOffFrequency); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CurrentAccelerationCutOffFrequency); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerDriverStatusGet : Read positioner driver status + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * int *DriverStatus + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerDriverStatusGet (int SocketIndex, char * PositionerName, int * DriverStatus) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerDriverStatusGet (%s,int *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", DriverStatus); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerDriverStatusStringGet : Return the positioner driver status string corresponding to the positioner error code + * + * - Parameters : + * int SocketIndex + * int PositionerDriverStatus + * char *PositionerDriverStatusString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerDriverStatusStringGet (int SocketIndex, int PositionerDriverStatus, char * PositionerDriverStatusString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerDriverStatusStringGet (%d,char *)", PositionerDriverStatus); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (PositionerDriverStatusString, pt); + ptNext = strchr (PositionerDriverStatusString, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerEncoderAmplitudeValuesGet : Read analog interpolated encoder amplitude values + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *CalibrationSinusAmplitude + * double *CurrentSinusAmplitude + * double *CalibrationCosinusAmplitude + * double *CurrentCosinusAmplitude + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerEncoderAmplitudeValuesGet (int SocketIndex, char * PositionerName, double * CalibrationSinusAmplitude, double * CurrentSinusAmplitude, double * CalibrationCosinusAmplitude, double * CurrentCosinusAmplitude) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerEncoderAmplitudeValuesGet (%s,double *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CalibrationSinusAmplitude); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CurrentSinusAmplitude); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CalibrationCosinusAmplitude); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CurrentCosinusAmplitude); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerEncoderCalibrationParametersGet : Read analog interpolated encoder calibration parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *SinusOffset + * double *CosinusOffset + * double *DifferentialGain + * double *PhaseCompensation + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerEncoderCalibrationParametersGet (int SocketIndex, char * PositionerName, double * SinusOffset, double * CosinusOffset, double * DifferentialGain, double * PhaseCompensation) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerEncoderCalibrationParametersGet (%s,double *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SinusOffset); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CosinusOffset); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", DifferentialGain); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", PhaseCompensation); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerErrorGet : Read and clear positioner error code + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * int *ErrorCode + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerErrorGet (int SocketIndex, char * PositionerName, int * ErrorCode) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerErrorGet (%s,int *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", ErrorCode); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerErrorRead : Read only positioner error code without clear it + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * int *ErrorCode + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerErrorRead (int SocketIndex, char * PositionerName, int * ErrorCode) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerErrorRead (%s,int *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", ErrorCode); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerErrorStringGet : Return the positioner status string corresponding to the positioner error code + * + * - Parameters : + * int SocketIndex + * int PositionerErrorCode + * char *PositionerErrorString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerErrorStringGet (int SocketIndex, int PositionerErrorCode, char * PositionerErrorString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerErrorStringGet (%d,char *)", PositionerErrorCode); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (PositionerErrorString, pt); + ptNext = strchr (PositionerErrorString, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerHardwareStatusGet : Read positioner hardware status + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * int *HardwareStatus + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerHardwareStatusGet (int SocketIndex, char * PositionerName, int * HardwareStatus) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerHardwareStatusGet (%s,int *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", HardwareStatus); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerHardwareStatusStringGet : Return the positioner hardware status string corresponding to the positioner error code + * + * - Parameters : + * int SocketIndex + * int PositionerHardwareStatus + * char *PositionerHardwareStatusString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerHardwareStatusStringGet (int SocketIndex, int PositionerHardwareStatus, char * PositionerHardwareStatusString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerHardwareStatusStringGet (%d,char *)", PositionerHardwareStatus); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (PositionerHardwareStatusString, pt); + ptNext = strchr (PositionerHardwareStatusString, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerHardInterpolatorFactorGet : Get hard interpolator parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * int *InterpolationFactor + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerHardInterpolatorFactorGet (int SocketIndex, char * PositionerName, int * InterpolationFactor) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerHardInterpolatorFactorGet (%s,int *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", InterpolationFactor); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerHardInterpolatorFactorSet : Set hard interpolator parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * int InterpolationFactor + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerHardInterpolatorFactorSet (int SocketIndex, char * PositionerName, int InterpolationFactor) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerHardInterpolatorFactorSet (%s,%d)", PositionerName, InterpolationFactor); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerMaximumVelocityAndAccelerationGet : Return maximum velocity and acceleration of the positioner + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *MaximumVelocity + * double *MaximumAcceleration + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerMaximumVelocityAndAccelerationGet (int SocketIndex, char * PositionerName, double * MaximumVelocity, double * MaximumAcceleration) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerMaximumVelocityAndAccelerationGet (%s,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MaximumVelocity); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MaximumAcceleration); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerMotionDoneGet : Read motion done parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *PositionWindow + * double *VelocityWindow + * double *CheckingTime + * double *MeanPeriod + * double *TimeOut + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerMotionDoneGet (int SocketIndex, char * PositionerName, double * PositionWindow, double * VelocityWindow, double * CheckingTime, double * MeanPeriod, double * TimeOut) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerMotionDoneGet (%s,double *,double *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", PositionWindow); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", VelocityWindow); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CheckingTime); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MeanPeriod); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", TimeOut); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerMotionDoneSet : Update motion done parameters + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double PositionWindow + * double VelocityWindow + * double CheckingTime + * double MeanPeriod + * double TimeOut + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerMotionDoneSet (int SocketIndex, char * PositionerName, double PositionWindow, double VelocityWindow, double CheckingTime, double MeanPeriod, double TimeOut) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerMotionDoneSet (%s,%.13g,%.13g,%.13g,%.13g,%.13g)", PositionerName, PositionWindow, VelocityWindow, CheckingTime, MeanPeriod, TimeOut); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerSGammaExactVelocityAjustedDisplacementGet : Return adjusted displacement to get exact velocity + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double DesiredDisplacement + * double *AdjustedDisplacement + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerSGammaExactVelocityAjustedDisplacementGet (int SocketIndex, char * PositionerName, double DesiredDisplacement, double * AdjustedDisplacement) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerSGammaExactVelocityAjustedDisplacementGet (%s,%.13g,double *)", PositionerName, DesiredDisplacement); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", AdjustedDisplacement); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerSGammaParametersGet : Read dynamic parameters for one axe of a group for a future displacement + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *Velocity + * double *Acceleration + * double *MinimumTjerkTime + * double *MaximumTjerkTime + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerSGammaParametersGet (int SocketIndex, char * PositionerName, double * Velocity, double * Acceleration, double * MinimumTjerkTime, double * MaximumTjerkTime) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerSGammaParametersGet (%s,double *,double *,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Velocity); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Acceleration); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MinimumTjerkTime); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MaximumTjerkTime); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerSGammaParametersSet : Update dynamic parameters for one axe of a group for a future displacement + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double Velocity + * double Acceleration + * double MinimumTjerkTime + * double MaximumTjerkTime + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerSGammaParametersSet (int SocketIndex, char * PositionerName, double Velocity, double Acceleration, double MinimumTjerkTime, double MaximumTjerkTime) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerSGammaParametersSet (%s,%.13g,%.13g,%.13g,%.13g)", PositionerName, Velocity, Acceleration, MinimumTjerkTime, MaximumTjerkTime); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerSGammaPreviousMotionTimesGet : Read SettingTime and SettlingTime + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *SettingTime + * double *SettlingTime + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerSGammaPreviousMotionTimesGet (int SocketIndex, char * PositionerName, double * SettingTime, double * SettlingTime) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerSGammaPreviousMotionTimesGet (%s,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SettingTime); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SettlingTime); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerStageParameterGet : Return the stage parameter + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * char *ParameterName + * char *ParameterValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerStageParameterGet (int SocketIndex, char * PositionerName, char * ParameterName, char * ParameterValue) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerStageParameterGet (%s,%s,char *)", PositionerName, ParameterName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ParameterValue, pt); + ptNext = strchr (ParameterValue, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerStageParameterSet : Save the stage parameter + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * char *ParameterName + * char *ParameterValue + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerStageParameterSet (int SocketIndex, char * PositionerName, char * ParameterName, char * ParameterValue) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerStageParameterSet (%s,%s,%s)", PositionerName, ParameterName, ParameterValue); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerUserTravelLimitsGet : Read UserMinimumTarget and UserMaximumTarget + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double *UserMinimumTarget + * double *UserMaximumTarget + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerUserTravelLimitsGet (int SocketIndex, char * PositionerName, double * UserMinimumTarget, double * UserMaximumTarget) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerUserTravelLimitsGet (%s,double *,double *)", PositionerName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserMinimumTarget); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserMaximumTarget); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerUserTravelLimitsSet : Update UserMinimumTarget and UserMaximumTarget + * + * - Parameters : + * int SocketIndex + * char *PositionerName + * double UserMinimumTarget + * double UserMaximumTarget + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerUserTravelLimitsSet (int SocketIndex, char * PositionerName, double UserMinimumTarget, double UserMaximumTarget) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerUserTravelLimitsSet (%s,%.13g,%.13g)", PositionerName, UserMinimumTarget, UserMaximumTarget); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * HexapodMoveAbsolute : Hexapod absolute move in a specific coordinate system + * + * - Parameters : + * int SocketIndex + * char *GroupName + * char *CoordinateSystem + * double X + * double Y + * double Z + * double U + * double V + * double W + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPHexapodMoveAbsolute (int SocketIndex, char * GroupName, char * CoordinateSystem, double X, double Y, double Z, double U, double V, double W) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "HexapodMoveAbsolute (%s,%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", GroupName, CoordinateSystem, X, Y, Z, U, V, W); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * HexapodMoveIncremental : Hexapod incremental move in a specific coordinate system + * + * - Parameters : + * int SocketIndex + * char *GroupName + * char *CoordinateSystem + * double dX + * double dY + * double dZ + * double dU + * double dV + * double dW + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPHexapodMoveIncremental (int SocketIndex, char * GroupName, char * CoordinateSystem, double dX, double dY, double dZ, double dU, double dV, double dW) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "HexapodMoveIncremental (%s,%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", GroupName, CoordinateSystem, dX, dY, dZ, dU, dV, dW); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * HexapodCoordinatesGet : Get coordinates in a specific coordinate system of a point specified in another coordinate system + * + * - Parameters : + * int SocketIndex + * char *GroupName + * char *CoordinateSystemIn + * char *CoordinateSystemOut + * double Xin + * double Yin + * double Zin + * double Uin + * double Vin + * double Win + * double *Xout + * double *Yout + * double *Zout + * double *Uout + * double *Vout + * double *Wout + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPHexapodCoordinatesGet (int SocketIndex, char * GroupName, char * CoordinateSystemIn, char * CoordinateSystemOut, double Xin, double Yin, double Zin, double Uin, double Vin, double Win, double * Xout, double * Yout, double * Zout, double * Uout, double * Vout, double * Wout) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "HexapodCoordinatesGet (%s,%s,%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g,double *,double *,double *,double *,double *,double *)", GroupName, CoordinateSystemIn, CoordinateSystemOut, Xin, Yin, Zin, Uin, Vin, Win); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Xout); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Yout); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Zout); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Uout); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Vout); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Wout); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * HexapodCoordinateSystemSet : Modify the position of a coordinate system + * + * - Parameters : + * int SocketIndex + * char *GroupName + * char *CoordinateSystem + * double X + * double Y + * double Z + * double U + * double V + * double W + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPHexapodCoordinateSystemSet (int SocketIndex, char * GroupName, char * CoordinateSystem, double X, double Y, double Z, double U, double V, double W) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "HexapodCoordinateSystemSet (%s,%s,%.13g,%.13g,%.13g,%.13g,%.13g,%.13g)", GroupName, CoordinateSystem, X, Y, Z, U, V, W); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * HexapodCoordinateSystemGet : Get the position of a coordinate system + * + * - Parameters : + * int SocketIndex + * char *GroupName + * char *CoordinateSystem + * double *X + * double *Y + * double *Z + * double *U + * double *V + * double *W + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPHexapodCoordinateSystemGet (int SocketIndex, char * GroupName, char * CoordinateSystem, double * X, double * Y, double * Z, double * U, double * V, double * W) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "HexapodCoordinateSystemGet (%s,%s,double *,double *,double *,double *,double *,double *)", GroupName, CoordinateSystem); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", X); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Y); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", Z); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", U); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", V); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", W); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * OptionalModuleExecute : Execute an optional module + * + * - Parameters : + * int SocketIndex + * char *ModuleFileName + * char *TaskName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPOptionalModuleExecute (int SocketIndex, char * ModuleFileName, char * TaskName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "OptionalModuleExecute (%s,%s)", ModuleFileName, TaskName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * OptionalModuleKill : Kill an optional module + * + * - Parameters : + * int SocketIndex + * char *TaskName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPOptionalModuleKill (int SocketIndex, char * TaskName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "OptionalModuleKill (%s)", TaskName); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ControllerStatusGet : Read controller current status + * + * - Parameters : + * int SocketIndex + * int *ControllerStatus + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPControllerStatusGet (int SocketIndex, int * ControllerStatus) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "ControllerStatusGet (int *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%d", ControllerStatus); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ControllerStatusStringGet : Return the controller status string corresponding to the controller status code + * + * - Parameters : + * int SocketIndex + * int ControllerStatusCode + * char *ControllerStatusString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPControllerStatusStringGet (int SocketIndex, int ControllerStatusCode, char * ControllerStatusString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "ControllerStatusStringGet (%d,char *)", ControllerStatusCode); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ControllerStatusString, pt); + ptNext = strchr (ControllerStatusString, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EEPROMCIESet : Set CIE EEPROM reference string + * + * - Parameters : + * int SocketIndex + * int CardNumber + * char *ReferenceString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEEPROMCIESet (int SocketIndex, int CardNumber, char * ReferenceString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EEPROMCIESet (%d,%s)", CardNumber, ReferenceString); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EEPROMDACOffsetCIESet : Set CIE DAC offsets + * + * - Parameters : + * int SocketIndex + * int PlugNumber + * double DAC1Offset + * double DAC2Offset + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEEPROMDACOffsetCIESet (int SocketIndex, int PlugNumber, double DAC1Offset, double DAC2Offset) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EEPROMDACOffsetCIESet (%d,%.13g,%.13g)", PlugNumber, DAC1Offset, DAC2Offset); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EEPROMDriverSet : Set Driver EEPROM reference string + * + * - Parameters : + * int SocketIndex + * int PlugNumber + * char *ReferenceString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEEPROMDriverSet (int SocketIndex, int PlugNumber, char * ReferenceString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EEPROMDriverSet (%d,%s)", PlugNumber, ReferenceString); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EEPROMINTSet : Set INT EEPROM reference string + * + * - Parameters : + * int SocketIndex + * int CardNumber + * char *ReferenceString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEEPROMINTSet (int SocketIndex, int CardNumber, char * ReferenceString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "EEPROMINTSet (%d,%s)", CardNumber, ReferenceString); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * CPUCoreAndBoardSupplyVoltagesGet : Get power informations + * + * - Parameters : + * int SocketIndex + * double *VoltageCPUCore + * double *SupplyVoltage1P5V + * double *SupplyVoltage3P3V + * double *SupplyVoltage5V + * double *SupplyVoltage12V + * double *SupplyVoltageM12V + * double *SupplyVoltageM5V + * double *SupplyVoltage5VSB + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPCPUCoreAndBoardSupplyVoltagesGet (int SocketIndex, double * VoltageCPUCore, double * SupplyVoltage1P5V, double * SupplyVoltage3P3V, double * SupplyVoltage5V, double * SupplyVoltage12V, double * SupplyVoltageM12V, double * SupplyVoltageM5V, double * SupplyVoltage5VSB) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "CPUCoreAndBoardSupplyVoltagesGet (double *,double *,double *,double *,double *,double *,double *,double *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", VoltageCPUCore); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage1P5V); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage3P3V); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage5V); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage12V); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SupplyVoltageM12V); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SupplyVoltageM5V); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", SupplyVoltage5VSB); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * CPUTemperatureAndFanSpeedGet : Get CPU temperature and fan speed + * + * - Parameters : + * int SocketIndex + * double *CPUTemperature + * double *CPUFanSpeed + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPCPUTemperatureAndFanSpeedGet (int SocketIndex, double * CPUTemperature, double * CPUFanSpeed) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "CPUTemperatureAndFanSpeedGet (double *,double *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CPUTemperature); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", CPUFanSpeed); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ActionListGet : Action list + * + * - Parameters : + * int SocketIndex + * char *ActionList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPActionListGet (int SocketIndex, char * ActionList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "ActionListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ActionList, pt); + ptNext = strchr (ActionList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ActionExtendedListGet : Action extended list + * + * - Parameters : + * int SocketIndex + * char *ActionList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPActionExtendedListGet (int SocketIndex, char * ActionList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "ActionExtendedListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ActionList, pt); + ptNext = strchr (ActionList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * APIExtendedListGet : API method list + * + * - Parameters : + * int SocketIndex + * char *Method + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPAPIExtendedListGet (int SocketIndex, char * Method) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); + + /* Convert to string */ + sprintf (ExecuteMethod, "APIExtendedListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (Method, pt); + ptNext = strchr (Method, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * APIListGet : API method list without extended API + * + * - Parameters : + * int SocketIndex + * char *Method + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPAPIListGet (int SocketIndex, char * Method) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); + + /* Convert to string */ + sprintf (ExecuteMethod, "APIListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (Method, pt); + ptNext = strchr (Method, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ErrorListGet : Error list + * + * - Parameters : + * int SocketIndex + * char *ErrorsList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPErrorListGet (int SocketIndex, char * ErrorsList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); + + /* Convert to string */ + sprintf (ExecuteMethod, "ErrorListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ErrorsList, pt); + ptNext = strchr (ErrorsList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * EventListGet : General event list + * + * - Parameters : + * int SocketIndex + * char *EventList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPEventListGet (int SocketIndex, char * EventList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "EventListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (EventList, pt); + ptNext = strchr (EventList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringListGet : Gathering type list + * + * - Parameters : + * int SocketIndex + * char *list + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringListGet (int SocketIndex, char * list) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (list, pt); + ptNext = strchr (list, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringExtendedListGet : Gathering type extended list + * + * - Parameters : + * int SocketIndex + * char *list + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringExtendedListGet (int SocketIndex, char * list) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringExtendedListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (list, pt); + ptNext = strchr (list, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringExternalListGet : External Gathering type list + * + * - Parameters : + * int SocketIndex + * char *list + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringExternalListGet (int SocketIndex, char * list) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringExternalListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (list, pt); + ptNext = strchr (list, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GroupStatusListGet : Group status list + * + * - Parameters : + * int SocketIndex + * char *GroupStatusList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGroupStatusListGet (int SocketIndex, char * GroupStatusList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); + + /* Convert to string */ + sprintf (ExecuteMethod, "GroupStatusListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (GroupStatusList, pt); + ptNext = strchr (GroupStatusList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * HardwareInternalListGet : Internal hardware list + * + * - Parameters : + * int SocketIndex + * char *InternalHardwareList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPHardwareInternalListGet (int SocketIndex, char * InternalHardwareList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); + + /* Convert to string */ + sprintf (ExecuteMethod, "HardwareInternalListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (InternalHardwareList, pt); + ptNext = strchr (InternalHardwareList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * HardwareDriverAndStageGet : Smart hardware + * + * - Parameters : + * int SocketIndex + * int PlugNumber + * char *DriverName + * char *StageName + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPHardwareDriverAndStageGet (int SocketIndex, int PlugNumber, char * DriverName, char * StageName) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_NOMINAL); + + /* Convert to string */ + sprintf (ExecuteMethod, "HardwareDriverAndStageGet (%d,char *,char *)", PlugNumber); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_NOMINAL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (DriverName, pt); + ptNext = strchr (DriverName, ','); + if (ptNext != NULL) *ptNext = '\0'; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (StageName, pt); + ptNext = strchr (StageName, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ObjectsListGet : Group name and positioner name + * + * - Parameters : + * int SocketIndex + * char *ObjectsList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPObjectsListGet (int SocketIndex, char * ObjectsList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_HUGE); + + /* Convert to string */ + sprintf (ExecuteMethod, "ObjectsListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_HUGE); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ObjectsList, pt); + ptNext = strchr (ObjectsList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerErrorListGet : Positioner error list + * + * - Parameters : + * int SocketIndex + * char *PositionerErrorList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerErrorListGet (int SocketIndex, char * PositionerErrorList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerErrorListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (PositionerErrorList, pt); + ptNext = strchr (PositionerErrorList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerHardwareStatusListGet : Positioner hardware status list + * + * - Parameters : + * int SocketIndex + * char *PositionerHardwareStatusList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerHardwareStatusListGet (int SocketIndex, char * PositionerHardwareStatusList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerHardwareStatusListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (PositionerHardwareStatusList, pt); + ptNext = strchr (PositionerHardwareStatusList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PositionerDriverStatusListGet : Positioner driver status list + * + * - Parameters : + * int SocketIndex + * char *PositionerDriverStatusList + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPositionerDriverStatusListGet (int SocketIndex, char * PositionerDriverStatusList) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "PositionerDriverStatusListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (PositionerDriverStatusList, pt); + ptNext = strchr (PositionerDriverStatusList, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ReferencingActionListGet : Get referencing action list + * + * - Parameters : + * int SocketIndex + * char *list + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPReferencingActionListGet (int SocketIndex, char * list) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "ReferencingActionListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (list, pt); + ptNext = strchr (list, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ReferencingSensorListGet : Get referencing sensor list + * + * - Parameters : + * int SocketIndex + * char *list + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPReferencingSensorListGet (int SocketIndex, char * list) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_BIG); + + /* Convert to string */ + sprintf (ExecuteMethod, "ReferencingSensorListGet (char *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_BIG); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (list, pt); + ptNext = strchr (list, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * GatheringUserDatasGet : Return UserDatas values + * + * - Parameters : + * int SocketIndex + * double *UserData1 + * double *UserData2 + * double *UserData3 + * double *UserData4 + * double *UserData5 + * double *UserData6 + * double *UserData7 + * double *UserData8 + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPGatheringUserDatasGet (int SocketIndex, double * UserData1, double * UserData2, double * UserData3, double * UserData4, double * UserData5, double * UserData6, double * UserData7, double * UserData8) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "GatheringUserDatasGet (double *,double *,double *,double *,double *,double *,double *,double *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserData1); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserData2); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserData3); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserData4); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserData5); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserData6); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserData7); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", UserData8); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ControllerMotionKernelPeriodMinMaxGet : Get controller motion kernel min/max periods + * + * - Parameters : + * int SocketIndex + * double *MinimumCorrectorPeriod + * double *MaximumCorrectorPeriod + * double *MinimumProfilerPeriod + * double *MaximumProfilerPeriod + * double *MinimumServitudesPeriod + * double *MaximumServitudesPeriod + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPControllerMotionKernelPeriodMinMaxGet (int SocketIndex, double * MinimumCorrectorPeriod, double * MaximumCorrectorPeriod, double * MinimumProfilerPeriod, double * MaximumProfilerPeriod, double * MinimumServitudesPeriod, double * MaximumServitudesPeriod) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "ControllerMotionKernelPeriodMinMaxGet (double *,double *,double *,double *,double *,double *)"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MinimumCorrectorPeriod); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MaximumCorrectorPeriod); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MinimumProfilerPeriod); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MaximumProfilerPeriod); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MinimumServitudesPeriod); + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) sscanf (pt, "%lf", MaximumServitudesPeriod); + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * ControllerMotionKernelPeriodMinMaxReset : Reset controller motion kernel min/max periods + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPControllerMotionKernelPeriodMinMaxReset (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "ControllerMotionKernelPeriodMinMaxReset ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * TestTCP : Test TCP/IP transfert + * + * - Parameters : + * int SocketIndex + * char *InputString + * char *ReturnString + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPTestTCP (int SocketIndex, char * InputString, char * ReturnString) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "TestTCP (%s,char *)", InputString); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (ret == 0) + { + char * pt; + char * ptNext; + + pt = ReturnedValue; + ptNext = NULL; + if (pt != NULL) pt = strchr (pt, ','); + if (pt != NULL) pt++; + if (pt != NULL) strcpy (ReturnString, pt); + ptNext = strchr (ReturnString, ','); + if (ptNext != NULL) *ptNext = '\0'; + } + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + +/*********************************************************************** + * PrepareForUpdate : Kill QNX processes for firmware update + * + * - Parameters : + * int SocketIndex + * - Return : + * int errorCode + ***********************************************************************/ +int __stdcall HXPPrepareForUpdate (int SocketIndex) +{ + int ret = -1; + char ExecuteMethod[SIZE_EXECUTE_METHOD]; + char *ReturnedValue = (char *) malloc (sizeof(char) * SIZE_SMALL); + + /* Convert to string */ + sprintf (ExecuteMethod, "PrepareForUpdate ()"); + + /* Send this string and wait return function from controller */ + /* return function : ==0 -> OK ; < 0 -> NOK */ + SendAndReceive (SocketIndex, ExecuteMethod, ReturnedValue, SIZE_SMALL); + if (strlen (ReturnedValue) > 0) + sscanf (ReturnedValue, "%i", &ret); + + /* Get the returned values in the out parameters */ + if (NULL != ReturnedValue) + free (ReturnedValue); + + return (ret); +} + + + +#ifdef __cplusplus +} +#endif diff --git a/motorApp/NewportSrc/hxp_drivers.h b/motorApp/NewportSrc/hxp_drivers.h index 45c6bc227..1928dbb6a 100644 --- a/motorApp/NewportSrc/hxp_drivers.h +++ b/motorApp/NewportSrc/hxp_drivers.h @@ -1,181 +1,181 @@ -/************************************************* - * XPS_API.h * - * * - * Description: * - * XPS functions * - *************************************************/ - -#ifdef _WIN32 - #ifndef DLL - #ifdef _DLL /* _DLL is defined by EPICS if we are being compiled to call DLLs */ - #define DLL _declspec(dllimport) - #else - #define DLL - #endif - #endif -#else - #define DLL - #define __stdcall -#endif -#ifdef __cplusplus -extern "C" -{ -#else -#typedef int bool; /* C does not know bool, only C++ */ -#endif - - -DLL int __stdcall HXPTCP_ConnectToServer(char *Ip_Address, int Ip_Port, double TimeOut); -DLL void __stdcall HXPTCP_SetTimeout(int SocketIndex, double Timeout); -DLL void __stdcall HXPTCP_CloseSocket(int SocketIndex); -DLL char * __stdcall HXPTCP_GetError(int SocketIndex); -DLL char * __stdcall HXPGetLibraryVersion(void); -DLL int __stdcall HXPControllerMotionKernelTimeLoadGet (int SocketIndex, double * CPUTotalLoadRatio, double * CPUCorrectorLoadRatio, double * CPUProfilerLoadRatio, double * CPUServitudesLoadRatio); /* Get controller motion kernel time load */ -DLL int __stdcall HXPElapsedTimeGet (int SocketIndex, double * ElapsedTime); /* Return elapsed time from controller power on */ -DLL int __stdcall HXPErrorStringGet (int SocketIndex, int ErrorCode, char * ErrorString); /* Return the error string corresponding to the error code */ -DLL int __stdcall HXPFirmwareVersionGet (int SocketIndex, char * Version); /* Return firmware version */ -DLL int __stdcall HXPTCLScriptExecute (int SocketIndex, char * TCLFileName, char * TaskName, char * ParametersList); /* Execute a TCL script from a TCL file */ -DLL int __stdcall HXPTCLScriptExecuteAndWait (int SocketIndex, char * TCLFileName, char * TaskName, char * InputParametersList, char * OutputParametersList); /* Execute a TCL script from a TCL file and wait the end of execution to return */ -DLL int __stdcall HXPTCLScriptKill (int SocketIndex, char * TaskName); /* Kill TCL Task */ -DLL int __stdcall HXPTimerGet (int SocketIndex, char * TimerName, int * FrequencyTicks); /* Get a timer */ -DLL int __stdcall HXPTimerSet (int SocketIndex, char * TimerName, int FrequencyTicks); /* Set a timer */ -DLL int __stdcall HXPReboot (int SocketIndex); /* Reboot the controller */ -DLL int __stdcall HXPLogin (int SocketIndex, char * Name, char * Password); /* Log in */ -DLL int __stdcall HXPCloseAllOtherSockets (int SocketIndex); /* Close all socket beside the one used to send this command */ -DLL int __stdcall HXPEventAdd (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter, char * ActionName, char * ActionParameter1, char * ActionParameter2, char * ActionParameter3); /* ** OBSOLETE ** Add an event */ -DLL int __stdcall HXPEventGet (int SocketIndex, char * PositionerName, char * EventsAndActionsList); /* ** OBSOLETE ** Read events and actions list */ -DLL int __stdcall HXPEventRemove (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter); /* ** OBSOLETE ** Delete an event */ -DLL int __stdcall HXPEventWait (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter); /* ** OBSOLETE ** Wait an event */ -DLL int __stdcall HXPEventExtendedConfigurationTriggerSet (int SocketIndex, int NbElements, char * ExtendedEventNameList, char * EventParameter1List, char * EventParameter2List, char * EventParameter3List, char * EventParameter4List); /* Configure one or several events */ -DLL int __stdcall HXPEventExtendedConfigurationTriggerGet (int SocketIndex, char * EventTriggerConfiguration); /* Read the event configuration */ -DLL int __stdcall HXPEventExtendedConfigurationActionSet (int SocketIndex, int NbElements, char * ExtendedActionNameList, char * ActionParameter1List, char * ActionParameter2List, char * ActionParameter3List, char * ActionParameter4List); /* Configure one or several actions */ -DLL int __stdcall HXPEventExtendedConfigurationActionGet (int SocketIndex, char * ActionConfiguration); /* Read the action configuration */ -DLL int __stdcall HXPEventExtendedStart (int SocketIndex, int * ID); /* Launch the last event and action configuration and return an ID */ -DLL int __stdcall HXPEventExtendedAllGet (int SocketIndex, char * EventActionConfigurations); /* Read all event and action configurations */ -DLL int __stdcall HXPEventExtendedGet (int SocketIndex, int ID, char * EventTriggerConfiguration, char * ActionConfiguration); /* Read the event and action configuration defined by ID */ -DLL int __stdcall HXPEventExtendedRemove (int SocketIndex, int ID); /* Remove the event and action configuration defined by ID */ -DLL int __stdcall HXPEventExtendedWait (int SocketIndex); /* Wait events from the last event configuration */ -DLL int __stdcall HXPGatheringConfigurationGet (int SocketIndex, char * Type); /* Read different mnemonique type */ -DLL int __stdcall HXPGatheringConfigurationSet (int SocketIndex, int NbElements, char * TypeList); /* Configuration acquisition */ -DLL int __stdcall HXPGatheringCurrentNumberGet (int SocketIndex, int * CurrentNumber, int * MaximumSamplesNumber); /* Maximum number of samples and current number during acquisition */ -DLL int __stdcall HXPGatheringStopAndSave (int SocketIndex); /* Stop acquisition and save data */ -DLL int __stdcall HXPGatheringDataAcquire (int SocketIndex); /* Acquire a configured data */ -DLL int __stdcall HXPGatheringDataGet (int SocketIndex, int IndexPoint, char * DataBufferLine); /* Get a data line from gathering buffer */ -DLL int __stdcall HXPGatheringReset (int SocketIndex); /* Empty the gathered data in memory to start new gathering from scratch */ -DLL int __stdcall HXPGatheringRun (int SocketIndex, int DataNumber, int Divisor); /* Start a new gathering */ -DLL int __stdcall HXPGatheringStop (int SocketIndex); /* Stop the data gathering (without saving to file) */ -DLL int __stdcall HXPGatheringExternalConfigurationSet (int SocketIndex, int NbElements, char * TypeList); /* Configuration acquisition */ -DLL int __stdcall HXPGatheringExternalConfigurationGet (int SocketIndex, char * Type); /* Read different mnemonique type */ -DLL int __stdcall HXPGatheringExternalCurrentNumberGet (int SocketIndex, int * CurrentNumber, int * MaximumSamplesNumber); /* Maximum number of samples and current number during acquisition */ -DLL int __stdcall HXPGatheringExternalStopAndSave (int SocketIndex); /* Stop acquisition and save data */ -DLL int __stdcall HXPGlobalArrayGet (int SocketIndex, int Number, char * ValueString); /* Get global array value */ -DLL int __stdcall HXPGlobalArraySet (int SocketIndex, int Number, char * ValueString); /* Set global array value */ -DLL int __stdcall HXPDoubleGlobalArrayGet (int SocketIndex, int Number, double * DoubleValue); /* Get double global array value */ -DLL int __stdcall HXPDoubleGlobalArraySet (int SocketIndex, int Number, double DoubleValue); /* Set double global array value */ -DLL int __stdcall HXPGPIOAnalogGet (int SocketIndex, int NbElements, char * GPIONameList, double AnalogValue[]); /* Read analog input or analog output for one or few input */ -DLL int __stdcall HXPGPIOAnalogSet (int SocketIndex, int NbElements, char * GPIONameList, double AnalogOutputValue[]); /* Set analog output for one or few output */ -DLL int __stdcall HXPGPIOAnalogGainGet (int SocketIndex, int NbElements, char * GPIONameList, int AnalogInputGainValue[]); /* Read analog input gain (1, 2, 4 or 8) for one or few input */ -DLL int __stdcall HXPGPIOAnalogGainSet (int SocketIndex, int NbElements, char * GPIONameList, int AnalogInputGainValue[]); /* Set analog input gain (1, 2, 4 or 8) for one or few input */ -DLL int __stdcall HXPGPIODigitalGet (int SocketIndex, char * GPIOName, unsigned short * DigitalValue); /* Read digital output or digital input */ -DLL int __stdcall HXPGPIODigitalSet (int SocketIndex, char * GPIOName, unsigned short Mask, unsigned short DigitalOutputValue); /* Set Digital Output for one or few output TTL */ -DLL int __stdcall HXPGroupCorrectorOutputGet (int SocketIndex, char * GroupName, int NbElements, double CorrectorOutput[]); /* Return corrector outputs */ -DLL int __stdcall HXPGroupHomeSearch (int SocketIndex, char * GroupName); /* Start home search sequence */ -DLL int __stdcall HXPGroupHomeSearchAndRelativeMove (int SocketIndex, char * GroupName, int NbElements, double TargetDisplacement[]); /* Start home search sequence and execute a displacement */ -DLL int __stdcall HXPGroupInitialize (int SocketIndex, char * GroupName); /* Start the initialization */ -DLL int __stdcall HXPGroupInitializeWithEncoderCalibration (int SocketIndex, char * GroupName); /* Start the initialization with encoder calibration */ -DLL int __stdcall HXPGroupKill (int SocketIndex, char * GroupName); /* Kill the group */ -DLL int __stdcall HXPGroupMoveAbort (int SocketIndex, char * GroupName); /* Abort a move */ -DLL int __stdcall HXPGroupMoveAbsolute (int SocketIndex, char * GroupName, int NbElements, double TargetPosition[]); /* Do an absolute move */ -DLL int __stdcall HXPGroupMoveRelative (int SocketIndex, char * GroupName, int NbElements, double TargetDisplacement[]); /* Do a relative move */ -DLL int __stdcall HXPGroupMotionDisable (int SocketIndex, char * GroupName); /* Set Motion disable on selected group */ -DLL int __stdcall HXPGroupMotionEnable (int SocketIndex, char * GroupName); /* Set Motion enable on selected group */ -DLL int __stdcall HXPGroupPositionCorrectedProfilerGet (int SocketIndex, char * GroupName, double PositionX, double PositionY, double * CorrectedProfilerPositionX, double * CorrectedProfilerPositionY); /* Return corrected profiler positions */ -DLL int __stdcall HXPGroupPositionCurrentGet (int SocketIndex, char * GroupName, int NbElements, double CurrentEncoderPosition[]); /* Return current positions */ -DLL int __stdcall HXPGroupPositionSetpointGet (int SocketIndex, char * GroupName, int NbElements, double SetPointPosition[]); /* Return setpoint positions */ -DLL int __stdcall HXPGroupPositionTargetGet (int SocketIndex, char * GroupName, int NbElements, double TargetPosition[]); /* Return target positions */ -DLL int __stdcall HXPGroupStatusGet (int SocketIndex, char * GroupName, int * Status); /* Return group status */ -DLL int __stdcall HXPGroupStatusStringGet (int SocketIndex, int GroupStatusCode, char * GroupStatusString); /* Return the group status string corresponding to the group status code */ -DLL int __stdcall HXPKillAll (int SocketIndex); /* Put all groups in 'Not initialized' state */ -DLL int __stdcall HXPRestartApplication (int SocketIndex); /* Restart the Controller */ -DLL int __stdcall HXPPositionerBacklashGet (int SocketIndex, char * PositionerName, double * BacklashValue, char * BacklaskStatus); /* Read backlash value and status */ -DLL int __stdcall HXPPositionerBacklashSet (int SocketIndex, char * PositionerName, double BacklashValue); /* Set backlash value */ -DLL int __stdcall HXPPositionerBacklashEnable (int SocketIndex, char * PositionerName); /* Enable the backlash */ -DLL int __stdcall HXPPositionerBacklashDisable (int SocketIndex, char * PositionerName); /* Disable the backlash */ -DLL int __stdcall HXPPositionerCorrectorNotchFiltersSet (int SocketIndex, char * PositionerName, double NotchFrequency1, double NotchBandwith1, double NotchGain1, double NotchFrequency2, double NotchBandwith2, double NotchGain2); /* Update filters parameters */ -DLL int __stdcall HXPPositionerCorrectorNotchFiltersGet (int SocketIndex, char * PositionerName, double * NotchFrequency1, double * NotchBandwith1, double * NotchGain1, double * NotchFrequency2, double * NotchBandwith2, double * NotchGain2); /* Read filters parameters */ -DLL int __stdcall HXPPositionerCorrectorPIDFFAccelerationSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainAcceleration); /* Update corrector parameters */ -DLL int __stdcall HXPPositionerCorrectorPIDFFAccelerationGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainAcceleration); /* Read corrector parameters */ -DLL int __stdcall HXPPositionerCorrectorPIDFFVelocitySet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainVelocity); /* Update corrector parameters */ -DLL int __stdcall HXPPositionerCorrectorPIDFFVelocityGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainVelocity); /* Read corrector parameters */ -DLL int __stdcall HXPPositionerCorrectorPIDDualFFVoltageSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainVelocity, double FeedForwardGainAcceleration, double Friction); /* Update corrector parameters */ -DLL int __stdcall HXPPositionerCorrectorPIDDualFFVoltageGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainVelocity, double * FeedForwardGainAcceleration, double * Friction); /* Read corrector parameters */ -DLL int __stdcall HXPPositionerCorrectorPIPositionSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double IntegrationTime); /* Update corrector parameters */ -DLL int __stdcall HXPPositionerCorrectorPIPositionGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * IntegrationTime); /* Read corrector parameters */ -DLL int __stdcall HXPPositionerCorrectorTypeGet (int SocketIndex, char * PositionerName, char * CorrectorType); /* Read corrector type */ -DLL int __stdcall HXPPositionerCurrentVelocityAccelerationFiltersSet (int SocketIndex, char * PositionerName, double CurrentVelocityCutOffFrequency, double CurrentAccelerationCutOffFrequency); /* Set current velocity and acceleration cut off frequencies */ -DLL int __stdcall HXPPositionerCurrentVelocityAccelerationFiltersGet (int SocketIndex, char * PositionerName, double * CurrentVelocityCutOffFrequency, double * CurrentAccelerationCutOffFrequency); /* Get current velocity and acceleration cut off frequencies */ -DLL int __stdcall HXPPositionerDriverStatusGet (int SocketIndex, char * PositionerName, int * DriverStatus); /* Read positioner driver status */ -DLL int __stdcall HXPPositionerDriverStatusStringGet (int SocketIndex, int PositionerDriverStatus, char * PositionerDriverStatusString); /* Return the positioner driver status string corresponding to the positioner error code */ -DLL int __stdcall HXPPositionerEncoderAmplitudeValuesGet (int SocketIndex, char * PositionerName, double * CalibrationSinusAmplitude, double * CurrentSinusAmplitude, double * CalibrationCosinusAmplitude, double * CurrentCosinusAmplitude); /* Read analog interpolated encoder amplitude values */ -DLL int __stdcall HXPPositionerEncoderCalibrationParametersGet (int SocketIndex, char * PositionerName, double * SinusOffset, double * CosinusOffset, double * DifferentialGain, double * PhaseCompensation); /* Read analog interpolated encoder calibration parameters */ -DLL int __stdcall HXPPositionerErrorGet (int SocketIndex, char * PositionerName, int * ErrorCode); /* Read and clear positioner error code */ -DLL int __stdcall HXPPositionerErrorRead (int SocketIndex, char * PositionerName, int * ErrorCode); /* Read only positioner error code without clear it */ -DLL int __stdcall HXPPositionerErrorStringGet (int SocketIndex, int PositionerErrorCode, char * PositionerErrorString); /* Return the positioner status string corresponding to the positioner error code */ -DLL int __stdcall HXPPositionerHardwareStatusGet (int SocketIndex, char * PositionerName, int * HardwareStatus); /* Read positioner hardware status */ -DLL int __stdcall HXPPositionerHardwareStatusStringGet (int SocketIndex, int PositionerHardwareStatus, char * PositionerHardwareStatusString); /* Return the positioner hardware status string corresponding to the positioner error code */ -DLL int __stdcall HXPPositionerHardInterpolatorFactorGet (int SocketIndex, char * PositionerName, int * InterpolationFactor); /* Get hard interpolator parameters */ -DLL int __stdcall HXPPositionerHardInterpolatorFactorSet (int SocketIndex, char * PositionerName, int InterpolationFactor); /* Set hard interpolator parameters */ -DLL int __stdcall HXPPositionerMaximumVelocityAndAccelerationGet (int SocketIndex, char * PositionerName, double * MaximumVelocity, double * MaximumAcceleration); /* Return maximum velocity and acceleration of the positioner */ -DLL int __stdcall HXPPositionerMotionDoneGet (int SocketIndex, char * PositionerName, double * PositionWindow, double * VelocityWindow, double * CheckingTime, double * MeanPeriod, double * TimeOut); /* Read motion done parameters */ -DLL int __stdcall HXPPositionerMotionDoneSet (int SocketIndex, char * PositionerName, double PositionWindow, double VelocityWindow, double CheckingTime, double MeanPeriod, double TimeOut); /* Update motion done parameters */ -DLL int __stdcall HXPPositionerSGammaExactVelocityAjustedDisplacementGet (int SocketIndex, char * PositionerName, double DesiredDisplacement, double * AdjustedDisplacement); /* Return adjusted displacement to get exact velocity */ -DLL int __stdcall HXPPositionerSGammaParametersGet (int SocketIndex, char * PositionerName, double * Velocity, double * Acceleration, double * MinimumTjerkTime, double * MaximumTjerkTime); /* Read dynamic parameters for one axe of a group for a future displacement */ -DLL int __stdcall HXPPositionerSGammaParametersSet (int SocketIndex, char * PositionerName, double Velocity, double Acceleration, double MinimumTjerkTime, double MaximumTjerkTime); /* Update dynamic parameters for one axe of a group for a future displacement */ -DLL int __stdcall HXPPositionerSGammaPreviousMotionTimesGet (int SocketIndex, char * PositionerName, double * SettingTime, double * SettlingTime); /* Read SettingTime and SettlingTime */ -DLL int __stdcall HXPPositionerStageParameterGet (int SocketIndex, char * PositionerName, char * ParameterName, char * ParameterValue); /* Return the stage parameter */ -DLL int __stdcall HXPPositionerStageParameterSet (int SocketIndex, char * PositionerName, char * ParameterName, char * ParameterValue); /* Save the stage parameter */ -DLL int __stdcall HXPPositionerUserTravelLimitsGet (int SocketIndex, char * PositionerName, double * UserMinimumTarget, double * UserMaximumTarget); /* Read UserMinimumTarget and UserMaximumTarget */ -DLL int __stdcall HXPPositionerUserTravelLimitsSet (int SocketIndex, char * PositionerName, double UserMinimumTarget, double UserMaximumTarget); /* Update UserMinimumTarget and UserMaximumTarget */ -DLL int __stdcall HXPHexapodMoveAbsolute (int SocketIndex, char * GroupName, char * CoordinateSystem, double X, double Y, double Z, double U, double V, double W); /* Hexapod absolute move in a specific coordinate system */ -DLL int __stdcall HXPHexapodMoveIncremental (int SocketIndex, char * GroupName, char * CoordinateSystem, double dX, double dY, double dZ, double dU, double dV, double dW); /* Hexapod incremental move in a specific coordinate system */ -DLL int __stdcall HXPHexapodCoordinatesGet (int SocketIndex, char * GroupName, char * CoordinateSystemIn, char * CoordinateSystemOut, double Xin, double Yin, double Zin, double Uin, double Vin, double Win, double * Xout, double * Yout, double * Zout, double * Uout, double * Vout, double * Wout); /* Get coordinates in a specific coordinate system of a point specified in another coordinate system */ -DLL int __stdcall HXPHexapodCoordinateSystemSet (int SocketIndex, char * GroupName, char * CoordinateSystem, double X, double Y, double Z, double U, double V, double W); /* Modify the position of a coordinate system */ -DLL int __stdcall HXPHexapodCoordinateSystemGet (int SocketIndex, char * GroupName, char * CoordinateSystem, double * X, double * Y, double * Z, double * U, double * V, double * W); /* Get the position of a coordinate system */ -DLL int __stdcall HXPOptionalModuleExecute (int SocketIndex, char * ModuleFileName, char * TaskName); /* Execute an optional module */ -DLL int __stdcall HXPOptionalModuleKill (int SocketIndex, char * TaskName); /* Kill an optional module */ -DLL int __stdcall HXPControllerStatusGet (int SocketIndex, int * ControllerStatus); /* Read controller current status */ -DLL int __stdcall HXPControllerStatusStringGet (int SocketIndex, int ControllerStatusCode, char * ControllerStatusString); /* Return the controller status string corresponding to the controller status code */ -DLL int __stdcall HXPEEPROMCIESet (int SocketIndex, int CardNumber, char * ReferenceString); /* Set CIE EEPROM reference string */ -DLL int __stdcall HXPEEPROMDACOffsetCIESet (int SocketIndex, int PlugNumber, double DAC1Offset, double DAC2Offset); /* Set CIE DAC offsets */ -DLL int __stdcall HXPEEPROMDriverSet (int SocketIndex, int PlugNumber, char * ReferenceString); /* Set Driver EEPROM reference string */ -DLL int __stdcall HXPEEPROMINTSet (int SocketIndex, int CardNumber, char * ReferenceString); /* Set INT EEPROM reference string */ -DLL int __stdcall HXPCPUCoreAndBoardSupplyVoltagesGet (int SocketIndex, double * VoltageCPUCore, double * SupplyVoltage1P5V, double * SupplyVoltage3P3V, double * SupplyVoltage5V, double * SupplyVoltage12V, double * SupplyVoltageM12V, double * SupplyVoltageM5V, double * SupplyVoltage5VSB); /* Get power informations */ -DLL int __stdcall HXPCPUTemperatureAndFanSpeedGet (int SocketIndex, double * CPUTemperature, double * CPUFanSpeed); /* Get CPU temperature and fan speed */ -DLL int __stdcall HXPActionListGet (int SocketIndex, char * ActionList); /* Action list */ -DLL int __stdcall HXPActionExtendedListGet (int SocketIndex, char * ActionList); /* Action extended list */ -DLL int __stdcall HXPAPIExtendedListGet (int SocketIndex, char * Method); /* API method list */ -DLL int __stdcall HXPAPIListGet (int SocketIndex, char * Method); /* API method list without extended API */ -DLL int __stdcall HXPErrorListGet (int SocketIndex, char * ErrorsList); /* Error list */ -DLL int __stdcall HXPEventListGet (int SocketIndex, char * EventList); /* General event list */ -DLL int __stdcall HXPGatheringListGet (int SocketIndex, char * list); /* Gathering type list */ -DLL int __stdcall HXPGatheringExtendedListGet (int SocketIndex, char * list); /* Gathering type extended list */ -DLL int __stdcall HXPGatheringExternalListGet (int SocketIndex, char * list); /* External Gathering type list */ -DLL int __stdcall HXPGroupStatusListGet (int SocketIndex, char * GroupStatusList); /* Group status list */ -DLL int __stdcall HXPHardwareInternalListGet (int SocketIndex, char * InternalHardwareList); /* Internal hardware list */ -DLL int __stdcall HXPHardwareDriverAndStageGet (int SocketIndex, int PlugNumber, char * DriverName, char * StageName); /* Smart hardware */ -DLL int __stdcall HXPObjectsListGet (int SocketIndex, char * ObjectsList); /* Group name and positioner name */ -DLL int __stdcall HXPPositionerErrorListGet (int SocketIndex, char * PositionerErrorList); /* Positioner error list */ -DLL int __stdcall HXPPositionerHardwareStatusListGet (int SocketIndex, char * PositionerHardwareStatusList); /* Positioner hardware status list */ -DLL int __stdcall HXPPositionerDriverStatusListGet (int SocketIndex, char * PositionerDriverStatusList); /* Positioner driver status list */ -DLL int __stdcall HXPReferencingActionListGet (int SocketIndex, char * list); /* Get referencing action list */ -DLL int __stdcall HXPReferencingSensorListGet (int SocketIndex, char * list); /* Get referencing sensor list */ -DLL int __stdcall HXPGatheringUserDatasGet (int SocketIndex, double * UserData1, double * UserData2, double * UserData3, double * UserData4, double * UserData5, double * UserData6, double * UserData7, double * UserData8); /* Return UserDatas values */ -DLL int __stdcall HXPControllerMotionKernelPeriodMinMaxGet (int SocketIndex, double * MinimumCorrectorPeriod, double * MaximumCorrectorPeriod, double * MinimumProfilerPeriod, double * MaximumProfilerPeriod, double * MinimumServitudesPeriod, double * MaximumServitudesPeriod); /* Get controller motion kernel min/max periods */ -DLL int __stdcall HXPControllerMotionKernelPeriodMinMaxReset (int SocketIndex); /* Reset controller motion kernel min/max periods */ -DLL int __stdcall HXPTestTCP (int SocketIndex, char * InputString, char * ReturnString); /* Test TCP/IP transfert */ -DLL int __stdcall HXPPrepareForUpdate (int SocketIndex); /* Kill QNX processes for firmware update */ - - -#ifdef __cplusplus -} -#endif +/************************************************* + * XPS_API.h * + * * + * Description: * + * XPS functions * + *************************************************/ + +#ifdef _WIN32 + #ifndef DLL + #ifdef _DLL /* _DLL is defined by EPICS if we are being compiled to call DLLs */ + #define DLL _declspec(dllimport) + #else + #define DLL + #endif + #endif +#else + #define DLL + #define __stdcall +#endif +#ifdef __cplusplus +extern "C" +{ +#else +#typedef int bool; /* C does not know bool, only C++ */ +#endif + + +DLL int __stdcall HXPTCP_ConnectToServer(char *Ip_Address, int Ip_Port, double TimeOut); +DLL void __stdcall HXPTCP_SetTimeout(int SocketIndex, double Timeout); +DLL void __stdcall HXPTCP_CloseSocket(int SocketIndex); +DLL char * __stdcall HXPTCP_GetError(int SocketIndex); +DLL char * __stdcall HXPGetLibraryVersion(void); +DLL int __stdcall HXPControllerMotionKernelTimeLoadGet (int SocketIndex, double * CPUTotalLoadRatio, double * CPUCorrectorLoadRatio, double * CPUProfilerLoadRatio, double * CPUServitudesLoadRatio); /* Get controller motion kernel time load */ +DLL int __stdcall HXPElapsedTimeGet (int SocketIndex, double * ElapsedTime); /* Return elapsed time from controller power on */ +DLL int __stdcall HXPErrorStringGet (int SocketIndex, int ErrorCode, char * ErrorString); /* Return the error string corresponding to the error code */ +DLL int __stdcall HXPFirmwareVersionGet (int SocketIndex, char * Version); /* Return firmware version */ +DLL int __stdcall HXPTCLScriptExecute (int SocketIndex, char * TCLFileName, char * TaskName, char * ParametersList); /* Execute a TCL script from a TCL file */ +DLL int __stdcall HXPTCLScriptExecuteAndWait (int SocketIndex, char * TCLFileName, char * TaskName, char * InputParametersList, char * OutputParametersList); /* Execute a TCL script from a TCL file and wait the end of execution to return */ +DLL int __stdcall HXPTCLScriptKill (int SocketIndex, char * TaskName); /* Kill TCL Task */ +DLL int __stdcall HXPTimerGet (int SocketIndex, char * TimerName, int * FrequencyTicks); /* Get a timer */ +DLL int __stdcall HXPTimerSet (int SocketIndex, char * TimerName, int FrequencyTicks); /* Set a timer */ +DLL int __stdcall HXPReboot (int SocketIndex); /* Reboot the controller */ +DLL int __stdcall HXPLogin (int SocketIndex, char * Name, char * Password); /* Log in */ +DLL int __stdcall HXPCloseAllOtherSockets (int SocketIndex); /* Close all socket beside the one used to send this command */ +DLL int __stdcall HXPEventAdd (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter, char * ActionName, char * ActionParameter1, char * ActionParameter2, char * ActionParameter3); /* ** OBSOLETE ** Add an event */ +DLL int __stdcall HXPEventGet (int SocketIndex, char * PositionerName, char * EventsAndActionsList); /* ** OBSOLETE ** Read events and actions list */ +DLL int __stdcall HXPEventRemove (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter); /* ** OBSOLETE ** Delete an event */ +DLL int __stdcall HXPEventWait (int SocketIndex, char * PositionerName, char * EventName, char * EventParameter); /* ** OBSOLETE ** Wait an event */ +DLL int __stdcall HXPEventExtendedConfigurationTriggerSet (int SocketIndex, int NbElements, char * ExtendedEventNameList, char * EventParameter1List, char * EventParameter2List, char * EventParameter3List, char * EventParameter4List); /* Configure one or several events */ +DLL int __stdcall HXPEventExtendedConfigurationTriggerGet (int SocketIndex, char * EventTriggerConfiguration); /* Read the event configuration */ +DLL int __stdcall HXPEventExtendedConfigurationActionSet (int SocketIndex, int NbElements, char * ExtendedActionNameList, char * ActionParameter1List, char * ActionParameter2List, char * ActionParameter3List, char * ActionParameter4List); /* Configure one or several actions */ +DLL int __stdcall HXPEventExtendedConfigurationActionGet (int SocketIndex, char * ActionConfiguration); /* Read the action configuration */ +DLL int __stdcall HXPEventExtendedStart (int SocketIndex, int * ID); /* Launch the last event and action configuration and return an ID */ +DLL int __stdcall HXPEventExtendedAllGet (int SocketIndex, char * EventActionConfigurations); /* Read all event and action configurations */ +DLL int __stdcall HXPEventExtendedGet (int SocketIndex, int ID, char * EventTriggerConfiguration, char * ActionConfiguration); /* Read the event and action configuration defined by ID */ +DLL int __stdcall HXPEventExtendedRemove (int SocketIndex, int ID); /* Remove the event and action configuration defined by ID */ +DLL int __stdcall HXPEventExtendedWait (int SocketIndex); /* Wait events from the last event configuration */ +DLL int __stdcall HXPGatheringConfigurationGet (int SocketIndex, char * Type); /* Read different mnemonique type */ +DLL int __stdcall HXPGatheringConfigurationSet (int SocketIndex, int NbElements, char * TypeList); /* Configuration acquisition */ +DLL int __stdcall HXPGatheringCurrentNumberGet (int SocketIndex, int * CurrentNumber, int * MaximumSamplesNumber); /* Maximum number of samples and current number during acquisition */ +DLL int __stdcall HXPGatheringStopAndSave (int SocketIndex); /* Stop acquisition and save data */ +DLL int __stdcall HXPGatheringDataAcquire (int SocketIndex); /* Acquire a configured data */ +DLL int __stdcall HXPGatheringDataGet (int SocketIndex, int IndexPoint, char * DataBufferLine); /* Get a data line from gathering buffer */ +DLL int __stdcall HXPGatheringReset (int SocketIndex); /* Empty the gathered data in memory to start new gathering from scratch */ +DLL int __stdcall HXPGatheringRun (int SocketIndex, int DataNumber, int Divisor); /* Start a new gathering */ +DLL int __stdcall HXPGatheringStop (int SocketIndex); /* Stop the data gathering (without saving to file) */ +DLL int __stdcall HXPGatheringExternalConfigurationSet (int SocketIndex, int NbElements, char * TypeList); /* Configuration acquisition */ +DLL int __stdcall HXPGatheringExternalConfigurationGet (int SocketIndex, char * Type); /* Read different mnemonique type */ +DLL int __stdcall HXPGatheringExternalCurrentNumberGet (int SocketIndex, int * CurrentNumber, int * MaximumSamplesNumber); /* Maximum number of samples and current number during acquisition */ +DLL int __stdcall HXPGatheringExternalStopAndSave (int SocketIndex); /* Stop acquisition and save data */ +DLL int __stdcall HXPGlobalArrayGet (int SocketIndex, int Number, char * ValueString); /* Get global array value */ +DLL int __stdcall HXPGlobalArraySet (int SocketIndex, int Number, char * ValueString); /* Set global array value */ +DLL int __stdcall HXPDoubleGlobalArrayGet (int SocketIndex, int Number, double * DoubleValue); /* Get double global array value */ +DLL int __stdcall HXPDoubleGlobalArraySet (int SocketIndex, int Number, double DoubleValue); /* Set double global array value */ +DLL int __stdcall HXPGPIOAnalogGet (int SocketIndex, int NbElements, char * GPIONameList, double AnalogValue[]); /* Read analog input or analog output for one or few input */ +DLL int __stdcall HXPGPIOAnalogSet (int SocketIndex, int NbElements, char * GPIONameList, double AnalogOutputValue[]); /* Set analog output for one or few output */ +DLL int __stdcall HXPGPIOAnalogGainGet (int SocketIndex, int NbElements, char * GPIONameList, int AnalogInputGainValue[]); /* Read analog input gain (1, 2, 4 or 8) for one or few input */ +DLL int __stdcall HXPGPIOAnalogGainSet (int SocketIndex, int NbElements, char * GPIONameList, int AnalogInputGainValue[]); /* Set analog input gain (1, 2, 4 or 8) for one or few input */ +DLL int __stdcall HXPGPIODigitalGet (int SocketIndex, char * GPIOName, unsigned short * DigitalValue); /* Read digital output or digital input */ +DLL int __stdcall HXPGPIODigitalSet (int SocketIndex, char * GPIOName, unsigned short Mask, unsigned short DigitalOutputValue); /* Set Digital Output for one or few output TTL */ +DLL int __stdcall HXPGroupCorrectorOutputGet (int SocketIndex, char * GroupName, int NbElements, double CorrectorOutput[]); /* Return corrector outputs */ +DLL int __stdcall HXPGroupHomeSearch (int SocketIndex, char * GroupName); /* Start home search sequence */ +DLL int __stdcall HXPGroupHomeSearchAndRelativeMove (int SocketIndex, char * GroupName, int NbElements, double TargetDisplacement[]); /* Start home search sequence and execute a displacement */ +DLL int __stdcall HXPGroupInitialize (int SocketIndex, char * GroupName); /* Start the initialization */ +DLL int __stdcall HXPGroupInitializeWithEncoderCalibration (int SocketIndex, char * GroupName); /* Start the initialization with encoder calibration */ +DLL int __stdcall HXPGroupKill (int SocketIndex, char * GroupName); /* Kill the group */ +DLL int __stdcall HXPGroupMoveAbort (int SocketIndex, char * GroupName); /* Abort a move */ +DLL int __stdcall HXPGroupMoveAbsolute (int SocketIndex, char * GroupName, int NbElements, double TargetPosition[]); /* Do an absolute move */ +DLL int __stdcall HXPGroupMoveRelative (int SocketIndex, char * GroupName, int NbElements, double TargetDisplacement[]); /* Do a relative move */ +DLL int __stdcall HXPGroupMotionDisable (int SocketIndex, char * GroupName); /* Set Motion disable on selected group */ +DLL int __stdcall HXPGroupMotionEnable (int SocketIndex, char * GroupName); /* Set Motion enable on selected group */ +DLL int __stdcall HXPGroupPositionCorrectedProfilerGet (int SocketIndex, char * GroupName, double PositionX, double PositionY, double * CorrectedProfilerPositionX, double * CorrectedProfilerPositionY); /* Return corrected profiler positions */ +DLL int __stdcall HXPGroupPositionCurrentGet (int SocketIndex, char * GroupName, int NbElements, double CurrentEncoderPosition[]); /* Return current positions */ +DLL int __stdcall HXPGroupPositionSetpointGet (int SocketIndex, char * GroupName, int NbElements, double SetPointPosition[]); /* Return setpoint positions */ +DLL int __stdcall HXPGroupPositionTargetGet (int SocketIndex, char * GroupName, int NbElements, double TargetPosition[]); /* Return target positions */ +DLL int __stdcall HXPGroupStatusGet (int SocketIndex, char * GroupName, int * Status); /* Return group status */ +DLL int __stdcall HXPGroupStatusStringGet (int SocketIndex, int GroupStatusCode, char * GroupStatusString); /* Return the group status string corresponding to the group status code */ +DLL int __stdcall HXPKillAll (int SocketIndex); /* Put all groups in 'Not initialized' state */ +DLL int __stdcall HXPRestartApplication (int SocketIndex); /* Restart the Controller */ +DLL int __stdcall HXPPositionerBacklashGet (int SocketIndex, char * PositionerName, double * BacklashValue, char * BacklaskStatus); /* Read backlash value and status */ +DLL int __stdcall HXPPositionerBacklashSet (int SocketIndex, char * PositionerName, double BacklashValue); /* Set backlash value */ +DLL int __stdcall HXPPositionerBacklashEnable (int SocketIndex, char * PositionerName); /* Enable the backlash */ +DLL int __stdcall HXPPositionerBacklashDisable (int SocketIndex, char * PositionerName); /* Disable the backlash */ +DLL int __stdcall HXPPositionerCorrectorNotchFiltersSet (int SocketIndex, char * PositionerName, double NotchFrequency1, double NotchBandwith1, double NotchGain1, double NotchFrequency2, double NotchBandwith2, double NotchGain2); /* Update filters parameters */ +DLL int __stdcall HXPPositionerCorrectorNotchFiltersGet (int SocketIndex, char * PositionerName, double * NotchFrequency1, double * NotchBandwith1, double * NotchGain1, double * NotchFrequency2, double * NotchBandwith2, double * NotchGain2); /* Read filters parameters */ +DLL int __stdcall HXPPositionerCorrectorPIDFFAccelerationSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainAcceleration); /* Update corrector parameters */ +DLL int __stdcall HXPPositionerCorrectorPIDFFAccelerationGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainAcceleration); /* Read corrector parameters */ +DLL int __stdcall HXPPositionerCorrectorPIDFFVelocitySet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainVelocity); /* Update corrector parameters */ +DLL int __stdcall HXPPositionerCorrectorPIDFFVelocityGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainVelocity); /* Read corrector parameters */ +DLL int __stdcall HXPPositionerCorrectorPIDDualFFVoltageSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double KD, double KS, double IntegrationTime, double DerivativeFilterCutOffFrequency, double GKP, double GKI, double GKD, double KForm, double FeedForwardGainVelocity, double FeedForwardGainAcceleration, double Friction); /* Update corrector parameters */ +DLL int __stdcall HXPPositionerCorrectorPIDDualFFVoltageGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * KD, double * KS, double * IntegrationTime, double * DerivativeFilterCutOffFrequency, double * GKP, double * GKI, double * GKD, double * KForm, double * FeedForwardGainVelocity, double * FeedForwardGainAcceleration, double * Friction); /* Read corrector parameters */ +DLL int __stdcall HXPPositionerCorrectorPIPositionSet (int SocketIndex, char * PositionerName, bool ClosedLoopStatus, double KP, double KI, double IntegrationTime); /* Update corrector parameters */ +DLL int __stdcall HXPPositionerCorrectorPIPositionGet (int SocketIndex, char * PositionerName, bool * ClosedLoopStatus, double * KP, double * KI, double * IntegrationTime); /* Read corrector parameters */ +DLL int __stdcall HXPPositionerCorrectorTypeGet (int SocketIndex, char * PositionerName, char * CorrectorType); /* Read corrector type */ +DLL int __stdcall HXPPositionerCurrentVelocityAccelerationFiltersSet (int SocketIndex, char * PositionerName, double CurrentVelocityCutOffFrequency, double CurrentAccelerationCutOffFrequency); /* Set current velocity and acceleration cut off frequencies */ +DLL int __stdcall HXPPositionerCurrentVelocityAccelerationFiltersGet (int SocketIndex, char * PositionerName, double * CurrentVelocityCutOffFrequency, double * CurrentAccelerationCutOffFrequency); /* Get current velocity and acceleration cut off frequencies */ +DLL int __stdcall HXPPositionerDriverStatusGet (int SocketIndex, char * PositionerName, int * DriverStatus); /* Read positioner driver status */ +DLL int __stdcall HXPPositionerDriverStatusStringGet (int SocketIndex, int PositionerDriverStatus, char * PositionerDriverStatusString); /* Return the positioner driver status string corresponding to the positioner error code */ +DLL int __stdcall HXPPositionerEncoderAmplitudeValuesGet (int SocketIndex, char * PositionerName, double * CalibrationSinusAmplitude, double * CurrentSinusAmplitude, double * CalibrationCosinusAmplitude, double * CurrentCosinusAmplitude); /* Read analog interpolated encoder amplitude values */ +DLL int __stdcall HXPPositionerEncoderCalibrationParametersGet (int SocketIndex, char * PositionerName, double * SinusOffset, double * CosinusOffset, double * DifferentialGain, double * PhaseCompensation); /* Read analog interpolated encoder calibration parameters */ +DLL int __stdcall HXPPositionerErrorGet (int SocketIndex, char * PositionerName, int * ErrorCode); /* Read and clear positioner error code */ +DLL int __stdcall HXPPositionerErrorRead (int SocketIndex, char * PositionerName, int * ErrorCode); /* Read only positioner error code without clear it */ +DLL int __stdcall HXPPositionerErrorStringGet (int SocketIndex, int PositionerErrorCode, char * PositionerErrorString); /* Return the positioner status string corresponding to the positioner error code */ +DLL int __stdcall HXPPositionerHardwareStatusGet (int SocketIndex, char * PositionerName, int * HardwareStatus); /* Read positioner hardware status */ +DLL int __stdcall HXPPositionerHardwareStatusStringGet (int SocketIndex, int PositionerHardwareStatus, char * PositionerHardwareStatusString); /* Return the positioner hardware status string corresponding to the positioner error code */ +DLL int __stdcall HXPPositionerHardInterpolatorFactorGet (int SocketIndex, char * PositionerName, int * InterpolationFactor); /* Get hard interpolator parameters */ +DLL int __stdcall HXPPositionerHardInterpolatorFactorSet (int SocketIndex, char * PositionerName, int InterpolationFactor); /* Set hard interpolator parameters */ +DLL int __stdcall HXPPositionerMaximumVelocityAndAccelerationGet (int SocketIndex, char * PositionerName, double * MaximumVelocity, double * MaximumAcceleration); /* Return maximum velocity and acceleration of the positioner */ +DLL int __stdcall HXPPositionerMotionDoneGet (int SocketIndex, char * PositionerName, double * PositionWindow, double * VelocityWindow, double * CheckingTime, double * MeanPeriod, double * TimeOut); /* Read motion done parameters */ +DLL int __stdcall HXPPositionerMotionDoneSet (int SocketIndex, char * PositionerName, double PositionWindow, double VelocityWindow, double CheckingTime, double MeanPeriod, double TimeOut); /* Update motion done parameters */ +DLL int __stdcall HXPPositionerSGammaExactVelocityAjustedDisplacementGet (int SocketIndex, char * PositionerName, double DesiredDisplacement, double * AdjustedDisplacement); /* Return adjusted displacement to get exact velocity */ +DLL int __stdcall HXPPositionerSGammaParametersGet (int SocketIndex, char * PositionerName, double * Velocity, double * Acceleration, double * MinimumTjerkTime, double * MaximumTjerkTime); /* Read dynamic parameters for one axe of a group for a future displacement */ +DLL int __stdcall HXPPositionerSGammaParametersSet (int SocketIndex, char * PositionerName, double Velocity, double Acceleration, double MinimumTjerkTime, double MaximumTjerkTime); /* Update dynamic parameters for one axe of a group for a future displacement */ +DLL int __stdcall HXPPositionerSGammaPreviousMotionTimesGet (int SocketIndex, char * PositionerName, double * SettingTime, double * SettlingTime); /* Read SettingTime and SettlingTime */ +DLL int __stdcall HXPPositionerStageParameterGet (int SocketIndex, char * PositionerName, char * ParameterName, char * ParameterValue); /* Return the stage parameter */ +DLL int __stdcall HXPPositionerStageParameterSet (int SocketIndex, char * PositionerName, char * ParameterName, char * ParameterValue); /* Save the stage parameter */ +DLL int __stdcall HXPPositionerUserTravelLimitsGet (int SocketIndex, char * PositionerName, double * UserMinimumTarget, double * UserMaximumTarget); /* Read UserMinimumTarget and UserMaximumTarget */ +DLL int __stdcall HXPPositionerUserTravelLimitsSet (int SocketIndex, char * PositionerName, double UserMinimumTarget, double UserMaximumTarget); /* Update UserMinimumTarget and UserMaximumTarget */ +DLL int __stdcall HXPHexapodMoveAbsolute (int SocketIndex, char * GroupName, char * CoordinateSystem, double X, double Y, double Z, double U, double V, double W); /* Hexapod absolute move in a specific coordinate system */ +DLL int __stdcall HXPHexapodMoveIncremental (int SocketIndex, char * GroupName, char * CoordinateSystem, double dX, double dY, double dZ, double dU, double dV, double dW); /* Hexapod incremental move in a specific coordinate system */ +DLL int __stdcall HXPHexapodCoordinatesGet (int SocketIndex, char * GroupName, char * CoordinateSystemIn, char * CoordinateSystemOut, double Xin, double Yin, double Zin, double Uin, double Vin, double Win, double * Xout, double * Yout, double * Zout, double * Uout, double * Vout, double * Wout); /* Get coordinates in a specific coordinate system of a point specified in another coordinate system */ +DLL int __stdcall HXPHexapodCoordinateSystemSet (int SocketIndex, char * GroupName, char * CoordinateSystem, double X, double Y, double Z, double U, double V, double W); /* Modify the position of a coordinate system */ +DLL int __stdcall HXPHexapodCoordinateSystemGet (int SocketIndex, char * GroupName, char * CoordinateSystem, double * X, double * Y, double * Z, double * U, double * V, double * W); /* Get the position of a coordinate system */ +DLL int __stdcall HXPOptionalModuleExecute (int SocketIndex, char * ModuleFileName, char * TaskName); /* Execute an optional module */ +DLL int __stdcall HXPOptionalModuleKill (int SocketIndex, char * TaskName); /* Kill an optional module */ +DLL int __stdcall HXPControllerStatusGet (int SocketIndex, int * ControllerStatus); /* Read controller current status */ +DLL int __stdcall HXPControllerStatusStringGet (int SocketIndex, int ControllerStatusCode, char * ControllerStatusString); /* Return the controller status string corresponding to the controller status code */ +DLL int __stdcall HXPEEPROMCIESet (int SocketIndex, int CardNumber, char * ReferenceString); /* Set CIE EEPROM reference string */ +DLL int __stdcall HXPEEPROMDACOffsetCIESet (int SocketIndex, int PlugNumber, double DAC1Offset, double DAC2Offset); /* Set CIE DAC offsets */ +DLL int __stdcall HXPEEPROMDriverSet (int SocketIndex, int PlugNumber, char * ReferenceString); /* Set Driver EEPROM reference string */ +DLL int __stdcall HXPEEPROMINTSet (int SocketIndex, int CardNumber, char * ReferenceString); /* Set INT EEPROM reference string */ +DLL int __stdcall HXPCPUCoreAndBoardSupplyVoltagesGet (int SocketIndex, double * VoltageCPUCore, double * SupplyVoltage1P5V, double * SupplyVoltage3P3V, double * SupplyVoltage5V, double * SupplyVoltage12V, double * SupplyVoltageM12V, double * SupplyVoltageM5V, double * SupplyVoltage5VSB); /* Get power informations */ +DLL int __stdcall HXPCPUTemperatureAndFanSpeedGet (int SocketIndex, double * CPUTemperature, double * CPUFanSpeed); /* Get CPU temperature and fan speed */ +DLL int __stdcall HXPActionListGet (int SocketIndex, char * ActionList); /* Action list */ +DLL int __stdcall HXPActionExtendedListGet (int SocketIndex, char * ActionList); /* Action extended list */ +DLL int __stdcall HXPAPIExtendedListGet (int SocketIndex, char * Method); /* API method list */ +DLL int __stdcall HXPAPIListGet (int SocketIndex, char * Method); /* API method list without extended API */ +DLL int __stdcall HXPErrorListGet (int SocketIndex, char * ErrorsList); /* Error list */ +DLL int __stdcall HXPEventListGet (int SocketIndex, char * EventList); /* General event list */ +DLL int __stdcall HXPGatheringListGet (int SocketIndex, char * list); /* Gathering type list */ +DLL int __stdcall HXPGatheringExtendedListGet (int SocketIndex, char * list); /* Gathering type extended list */ +DLL int __stdcall HXPGatheringExternalListGet (int SocketIndex, char * list); /* External Gathering type list */ +DLL int __stdcall HXPGroupStatusListGet (int SocketIndex, char * GroupStatusList); /* Group status list */ +DLL int __stdcall HXPHardwareInternalListGet (int SocketIndex, char * InternalHardwareList); /* Internal hardware list */ +DLL int __stdcall HXPHardwareDriverAndStageGet (int SocketIndex, int PlugNumber, char * DriverName, char * StageName); /* Smart hardware */ +DLL int __stdcall HXPObjectsListGet (int SocketIndex, char * ObjectsList); /* Group name and positioner name */ +DLL int __stdcall HXPPositionerErrorListGet (int SocketIndex, char * PositionerErrorList); /* Positioner error list */ +DLL int __stdcall HXPPositionerHardwareStatusListGet (int SocketIndex, char * PositionerHardwareStatusList); /* Positioner hardware status list */ +DLL int __stdcall HXPPositionerDriverStatusListGet (int SocketIndex, char * PositionerDriverStatusList); /* Positioner driver status list */ +DLL int __stdcall HXPReferencingActionListGet (int SocketIndex, char * list); /* Get referencing action list */ +DLL int __stdcall HXPReferencingSensorListGet (int SocketIndex, char * list); /* Get referencing sensor list */ +DLL int __stdcall HXPGatheringUserDatasGet (int SocketIndex, double * UserData1, double * UserData2, double * UserData3, double * UserData4, double * UserData5, double * UserData6, double * UserData7, double * UserData8); /* Return UserDatas values */ +DLL int __stdcall HXPControllerMotionKernelPeriodMinMaxGet (int SocketIndex, double * MinimumCorrectorPeriod, double * MaximumCorrectorPeriod, double * MinimumProfilerPeriod, double * MaximumProfilerPeriod, double * MinimumServitudesPeriod, double * MaximumServitudesPeriod); /* Get controller motion kernel min/max periods */ +DLL int __stdcall HXPControllerMotionKernelPeriodMinMaxReset (int SocketIndex); /* Reset controller motion kernel min/max periods */ +DLL int __stdcall HXPTestTCP (int SocketIndex, char * InputString, char * ReturnString); /* Test TCP/IP transfert */ +DLL int __stdcall HXPPrepareForUpdate (int SocketIndex); /* Kill QNX processes for firmware update */ + + +#ifdef __cplusplus +} +#endif diff --git a/motorApp/NewportSrc/hxp_errors.h b/motorApp/NewportSrc/hxp_errors.h index 52195524e..3bc40dc5b 100644 --- a/motorApp/NewportSrc/hxp_errors.h +++ b/motorApp/NewportSrc/hxp_errors.h @@ -1,98 +1,98 @@ -//////////////////////////////////////////////////////////////////// -// Created header file hxp_errors.h for XPS function errors -// - -// TCL interpretor error -#define ERR_TCL_INTERPRETOR_ERROR 1 - -// No error -#define SUCCESS 0 - -// XPS errors -#define ERR_TCL_INTERPRETOR_ERROR 1 -#define ERR_BUSY_SOCKET -1 -#define ERR_TCP_TIMEOUT -2 -#define ERR_STRING_TOO_LONG -3 -#define ERR_UNKNOWN_COMMAND -4 -#define ERR_POSITIONER_ERROR -5 -#define ERR_WRONG_FORMAT -7 -#define ERR_WRONG_OBJECT_TYPE -8 -#define ERR_WRONG_PARAMETERS_NUMBER -9 -#define ERR_WRONG_TYPE -10 -#define ERR_WRONG_TYPE_BIT_WORD -11 -#define ERR_WRONG_TYPE_BOOL -12 -#define ERR_WRONG_TYPE_CHAR -13 -#define ERR_WRONG_TYPE_DOUBLE -14 -#define ERR_WRONG_TYPE_INT -15 -#define ERR_WRONG_TYPE_UNSIGNEDINT -16 -#define ERR_PARAMETER_OUT_OF_RANGE -17 -#define ERR_POSITIONER_NAME -18 -#define ERR_GROUP_NAME -19 -#define ERR_FATAL_INIT -20 -#define ERR_IN_INITIALIZATION -21 -#define ERR_NOT_ALLOWED_ACTION -22 -#define ERR_POSITION_COMPARE_NOT_SET -23 -#define ERR_UNCOMPATIBLE -24 -#define ERR_FOLLOWING_ERROR -25 -#define ERR_EMERGENCY_SIGNAL -26 -#define ERR_GROUP_ABORT_MOTION -27 -#define ERR_GROUP_HOME_SEARCH_TIMEOUT -28 -#define ERR_MNEMOTYPEGATHERING -29 -#define ERR_GATHERING_NOT_STARTED -30 -#define ERR_HOME_OUT_RANGE -31 -#define ERR_GATHERING_NOT_CONFIGURED -32 -#define ERR_GROUP_MOTION_DONE_TIMEOUT -33 -#define ERR_TRAVEL_LIMITS -35 -#define ERR_UNKNOWN_TCL_FILE -36 -#define ERR_TCL_SCRIPT_KILL -38 -#define ERR_TCL_INTERPRETOR -37 -#define ERR_MNEMO_ACTION -39 -#define ERR_MNEMO_EVENT -40 -#define ERR_SLAVE_CONFIGURATION -41 -#define ERR_JOG_OUT_OF_RANGE -42 -#define ERR_GATHERING_RUNNING -43 -#define ERR_SLAVE -44 -#define ERR_END_OF_RUN -45 -#define ERR_NOT_ALLOWED_BACKLASH -46 -#define ERR_WRONG_TCL_TASKNAME -47 -#define ERR_BASE_VELOCITY -48 -#define ERR_GROUP_HOME_SEARCH_ZM_ERROR -49 -#define ERR_MOTOR_INITIALIZATION_ERROR -50 -#define ERR_WRITE_FILE -60 -#define ERR_READ_FILE -61 -#define ERR_TRAJ_ELEM_TYPE -62 -#define ERR_TRAJ_ELEM_LINE -65 -#define ERR_TRAJ_EMPTY -66 -#define ERR_TRAJ_VEL_LIMIT -68 -#define ERR_TRAJ_ACC_LIMIT -69 -#define ERR_TRAJ_FINAL_VELOCITY -70 -#define ERR_MSG_QUEUE -71 -#define ERR_TRAJ_INITIALIZATION -72 -#define ERR_END_OF_FILE -73 -#define ERR_READ_FILE_PARAMETER_KEY -74 -#define ERR_TRAJ_TIME -75 -#define ERR_EVENTS_NOT_CONFIGURED -80 -#define ERR_ACTIONS_NOT_CONFIGURED -81 -#define ERR_EVENT_BUFFER_FULL -82 -#define ERR_EVENT_ID_UNDEFINED -83 -#define ERR_HOME_SEARCH_GANTRY_TOLERANCE_ERROR -85 -#define ERR_OPTIONAL_EXTERNAL_MODULE_FILE -94 -#define ERR_OPTIONAL_EXTERNAL_MODULE_EXECUTE -95 -#define ERR_OPTIONAL_EXTERNAL_MODULE_KILL -96 -#define ERR_OPTIONAL_EXTERNAL_MODULE_LOAD -97 -#define ERR_OPTIONAL_EXTERNAL_MODULE_UNLOAD -98 -#define ERR_FATAL_EXTERNAL_MODULE_LOAD -99 -#define ERR_INTERNAL_ERROR -100 -#define ERR_RELAY_FEEDBACK_TEST_NO_OSCILLATION -101 -#define ERR_RELAY_FEEDBACK_TEST_SIGNAL_NOISY -102 -#define ERR_SIGNAL_POINTS_NOT_ENOUGH -103 -#define ERR_PID_TUNING_INITIALIZATION -104 -#define ERR_SCALING_CALIBRATION -105 -#define ERR_WRONG_USERNAME_OR_PASSWORD -106 -#define ERR_NEED_ADMINISTRATOR_RIGHTS -107 -#define ERR_SOCKET_CLOSED_BY_ADMIN -108 -#define ERR_NEED_TO_BE_HOMED_AT_LEAST_ONCE -109 -#define ERR_NOT_ALLOWED_FOR_GANTRY -110 -#define ERR_GATHERING_BUFFER_FULL -111 -#define ERR_BOTH_ENDS_OF_RUNS_ACTIVATED -113 -#define ERR_HARDWARE_FUNCTION_NOT_SUPPORTED -115 +//////////////////////////////////////////////////////////////////// +// Created header file hxp_errors.h for XPS function errors +// + +// TCL interpretor error +#define ERR_TCL_INTERPRETOR_ERROR 1 + +// No error +#define SUCCESS 0 + +// XPS errors +#define ERR_TCL_INTERPRETOR_ERROR 1 +#define ERR_BUSY_SOCKET -1 +#define ERR_TCP_TIMEOUT -2 +#define ERR_STRING_TOO_LONG -3 +#define ERR_UNKNOWN_COMMAND -4 +#define ERR_POSITIONER_ERROR -5 +#define ERR_WRONG_FORMAT -7 +#define ERR_WRONG_OBJECT_TYPE -8 +#define ERR_WRONG_PARAMETERS_NUMBER -9 +#define ERR_WRONG_TYPE -10 +#define ERR_WRONG_TYPE_BIT_WORD -11 +#define ERR_WRONG_TYPE_BOOL -12 +#define ERR_WRONG_TYPE_CHAR -13 +#define ERR_WRONG_TYPE_DOUBLE -14 +#define ERR_WRONG_TYPE_INT -15 +#define ERR_WRONG_TYPE_UNSIGNEDINT -16 +#define ERR_PARAMETER_OUT_OF_RANGE -17 +#define ERR_POSITIONER_NAME -18 +#define ERR_GROUP_NAME -19 +#define ERR_FATAL_INIT -20 +#define ERR_IN_INITIALIZATION -21 +#define ERR_NOT_ALLOWED_ACTION -22 +#define ERR_POSITION_COMPARE_NOT_SET -23 +#define ERR_UNCOMPATIBLE -24 +#define ERR_FOLLOWING_ERROR -25 +#define ERR_EMERGENCY_SIGNAL -26 +#define ERR_GROUP_ABORT_MOTION -27 +#define ERR_GROUP_HOME_SEARCH_TIMEOUT -28 +#define ERR_MNEMOTYPEGATHERING -29 +#define ERR_GATHERING_NOT_STARTED -30 +#define ERR_HOME_OUT_RANGE -31 +#define ERR_GATHERING_NOT_CONFIGURED -32 +#define ERR_GROUP_MOTION_DONE_TIMEOUT -33 +#define ERR_TRAVEL_LIMITS -35 +#define ERR_UNKNOWN_TCL_FILE -36 +#define ERR_TCL_SCRIPT_KILL -38 +#define ERR_TCL_INTERPRETOR -37 +#define ERR_MNEMO_ACTION -39 +#define ERR_MNEMO_EVENT -40 +#define ERR_SLAVE_CONFIGURATION -41 +#define ERR_JOG_OUT_OF_RANGE -42 +#define ERR_GATHERING_RUNNING -43 +#define ERR_SLAVE -44 +#define ERR_END_OF_RUN -45 +#define ERR_NOT_ALLOWED_BACKLASH -46 +#define ERR_WRONG_TCL_TASKNAME -47 +#define ERR_BASE_VELOCITY -48 +#define ERR_GROUP_HOME_SEARCH_ZM_ERROR -49 +#define ERR_MOTOR_INITIALIZATION_ERROR -50 +#define ERR_WRITE_FILE -60 +#define ERR_READ_FILE -61 +#define ERR_TRAJ_ELEM_TYPE -62 +#define ERR_TRAJ_ELEM_LINE -65 +#define ERR_TRAJ_EMPTY -66 +#define ERR_TRAJ_VEL_LIMIT -68 +#define ERR_TRAJ_ACC_LIMIT -69 +#define ERR_TRAJ_FINAL_VELOCITY -70 +#define ERR_MSG_QUEUE -71 +#define ERR_TRAJ_INITIALIZATION -72 +#define ERR_END_OF_FILE -73 +#define ERR_READ_FILE_PARAMETER_KEY -74 +#define ERR_TRAJ_TIME -75 +#define ERR_EVENTS_NOT_CONFIGURED -80 +#define ERR_ACTIONS_NOT_CONFIGURED -81 +#define ERR_EVENT_BUFFER_FULL -82 +#define ERR_EVENT_ID_UNDEFINED -83 +#define ERR_HOME_SEARCH_GANTRY_TOLERANCE_ERROR -85 +#define ERR_OPTIONAL_EXTERNAL_MODULE_FILE -94 +#define ERR_OPTIONAL_EXTERNAL_MODULE_EXECUTE -95 +#define ERR_OPTIONAL_EXTERNAL_MODULE_KILL -96 +#define ERR_OPTIONAL_EXTERNAL_MODULE_LOAD -97 +#define ERR_OPTIONAL_EXTERNAL_MODULE_UNLOAD -98 +#define ERR_FATAL_EXTERNAL_MODULE_LOAD -99 +#define ERR_INTERNAL_ERROR -100 +#define ERR_RELAY_FEEDBACK_TEST_NO_OSCILLATION -101 +#define ERR_RELAY_FEEDBACK_TEST_SIGNAL_NOISY -102 +#define ERR_SIGNAL_POINTS_NOT_ENOUGH -103 +#define ERR_PID_TUNING_INITIALIZATION -104 +#define ERR_SCALING_CALIBRATION -105 +#define ERR_WRONG_USERNAME_OR_PASSWORD -106 +#define ERR_NEED_ADMINISTRATOR_RIGHTS -107 +#define ERR_SOCKET_CLOSED_BY_ADMIN -108 +#define ERR_NEED_TO_BE_HOMED_AT_LEAST_ONCE -109 +#define ERR_NOT_ALLOWED_FOR_GANTRY -110 +#define ERR_GATHERING_BUFFER_FULL -111 +#define ERR_BOTH_ENDS_OF_RUNS_ACTIVATED -113 +#define ERR_HARDWARE_FUNCTION_NOT_SUPPORTED -115 From a94c24e4c76e9b18eab544120b75fcdf69730ecb Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 21 Aug 2015 22:40:35 +0100 Subject: [PATCH 023/232] Add MASTER_RELEASE.private --- configure/RELEASE | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/configure/RELEASE b/configure/RELEASE index 4915c302e..5c95195b8 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -1,6 +1,10 @@ +# top level master release and local private options include $(TOP)/../../../configure/MASTER_RELEASE +-include $(TOP)/../../../configure/MASTER_RELEASE.$(EPICS_HOST_ARCH) +-include $(TOP)/../../../configure/MASTER_RELEASE.private +-include $(TOP)/../../../configure/MASTER_RELEASE.private.$(EPICS_HOST_ARCH) -# overrdie module definitions here +# optional extra local definitions here -include $(TOP)/configure/RELEASE.private include $(TOP)/../../../ISIS_CONFIG.$(EPICS_HOST_ARCH) From 4f3f7b9a341f8cee12f10ca681c80b9cd8050c77 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 27 Aug 2015 14:06:36 +0100 Subject: [PATCH 024/232] Add runIOC.sh and relPaths.sh --- iocBoot/iocmotorSim/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/iocBoot/iocmotorSim/Makefile b/iocBoot/iocmotorSim/Makefile index 6812420ae..2ca9fa11f 100755 --- a/iocBoot/iocmotorSim/Makefile +++ b/iocBoot/iocmotorSim/Makefile @@ -4,7 +4,7 @@ ARCH = $(EPICS_HOST_ARCH) #ARCH = linux-x86_64 #ARCH = vxWorks-ppc604 TARGETS = cdCommands -TARGETS += envPaths dllPath.bat runIOC.bat +TARGETS += envPaths dllPath.bat runIOC.bat relPaths.sh runIOC.sh TARGETS += motor.substitutions.local motor.substitutions.local : motor.substitutions From 6d893ec82cfa99389d26e38e9561fb3215e014fc Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 27 Aug 2015 15:26:48 +0100 Subject: [PATCH 025/232] Add runIOC.sh and relPaths.sh --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 91cc72927..e476e7aeb 100644 --- a/.gitignore +++ b/.gitignore @@ -8,5 +8,7 @@ O.*/ envPaths cdCommands dllPath.bat -runIoc.bat +runIOC.bat +runIOC.sh +relPaths.sh iocBoot/iocmotorSim/motor.substitutions.local From b84e50a121bab197d0541b21169c6b430217e9a7 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 17 Feb 2016 18:15:52 +0000 Subject: [PATCH 026/232] Add ISIS :SP aliases --- motorApp/Db/asyn_motor.db | 4 ++++ motorApp/Db/basic_asyn_motor.db | 3 +++ motorApp/Db/basic_motor.db | 3 +++ motorApp/Db/motor.db | 4 ++++ motorApp/Db/pseudoMotor.db | 5 +++++ 5 files changed, 19 insertions(+) diff --git a/motorApp/Db/asyn_motor.db b/motorApp/Db/asyn_motor.db index f4811217b..7df37a55f 100644 --- a/motorApp/Db/asyn_motor.db +++ b/motorApp/Db/asyn_motor.db @@ -25,6 +25,10 @@ record(motor, "$(P)$(M)") { field(SDIS, "$(P)$(M)_able.VAL") } +# ISIS local aliases for genie_python +alias("$(P)$(M)", "$(P)$(M):SP") +alias("$(P)$(M)", "$(P)$(M):SP:RBV") + record(bo, "$(P)$(M)_able") { field(DESC, "motor enable") field(PINI, "YES") diff --git a/motorApp/Db/basic_asyn_motor.db b/motorApp/Db/basic_asyn_motor.db index c1146893f..6ae5db91d 100644 --- a/motorApp/Db/basic_asyn_motor.db +++ b/motorApp/Db/basic_asyn_motor.db @@ -19,3 +19,6 @@ record(motor,"$(P)$(M)") field(TWV,"1") } +# ISIS local aliases for genie_python +alias("$(P)$(M)", "$(P)$(M):SP") +alias("$(P)$(M)", "$(P)$(M):SP:RBV") diff --git a/motorApp/Db/basic_motor.db b/motorApp/Db/basic_motor.db index 98e55ec92..4aea3c4fa 100644 --- a/motorApp/Db/basic_motor.db +++ b/motorApp/Db/basic_motor.db @@ -19,3 +19,6 @@ grecord(motor,"$(P)$(M)") field(TWV,"1") } +# ISIS local aliases for genie_python +alias("$(P)$(M)", "$(P)$(M):SP") +alias("$(P)$(M)", "$(P)$(M):SP:RBV") diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 40df7fe4c..667adebca 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -24,6 +24,10 @@ record(motor, "$(P)$(M)") { field(SDIS, "$(P)$(M)_able.VAL") } +# ISIS local aliases for genie_python +alias("$(P)$(M)", "$(P)$(M):SP") +alias("$(P)$(M)", "$(P)$(M):SP:RBV") + record(bo, "$(P)$(M)_able") { field(DESC, "motor enable") field(PINI, "YES") diff --git a/motorApp/Db/pseudoMotor.db b/motorApp/Db/pseudoMotor.db index ad6bda90f..f961075ed 100644 --- a/motorApp/Db/pseudoMotor.db +++ b/motorApp/Db/pseudoMotor.db @@ -37,6 +37,11 @@ grecord(motor,"$(P)$(M)") field(FOFF,"Frozen") field(NTM,"NO") } + +# ISIS local aliases for genie_python +alias("$(P)$(M)", "$(P)$(M):SP") +alias("$(P)$(M)", "$(P)$(M):SP:RBV") + grecord(transform,"$(P)$(M)_ableput") { field(CLCB,"a") From 1411ede1cf3bc29839905ba3f82b62f6b3f1291f Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 9 Aug 2016 13:16:47 +0100 Subject: [PATCH 027/232] Add McLennan debugging option --- motorApp/MclennanSrc/Makefile | 2 +- motorApp/MclennanSrc/devMclennanMotor.dbd | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/motorApp/MclennanSrc/Makefile b/motorApp/MclennanSrc/Makefile index a4f0f48e9..5cc645837 100644 --- a/motorApp/MclennanSrc/Makefile +++ b/motorApp/MclennanSrc/Makefile @@ -4,7 +4,7 @@ include $(TOP)/configure/CONFIG # Both the following line, and a line in the *.dbd file, # must be uncommented to use diagnostic debugging messages. -#USR_CXXFLAGS += -DDEBUG +USR_CXXFLAGS += -DDEBUG DBD += devMclennanMotor.dbd diff --git a/motorApp/MclennanSrc/devMclennanMotor.dbd b/motorApp/MclennanSrc/devMclennanMotor.dbd index 9816012b0..ccf408f2a 100644 --- a/motorApp/MclennanSrc/devMclennanMotor.dbd +++ b/motorApp/MclennanSrc/devMclennanMotor.dbd @@ -2,6 +2,6 @@ device(motor,VME_IO,devPM304,"Mclennan PM304") driver(drvPM304) registrar(MclennanRegister) -#variable(drvPM304Debug) +variable(drvPM304Debug) From bc0179d6c41e966bb21b5f3c1ab5d131077f1b36 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 1 Sep 2016 16:44:35 +0100 Subject: [PATCH 028/232] Add motor status PV file and update make --- motorApp/Db/Makefile | 1 + motorApp/Db/motorStatus.db | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 33 insertions(+) create mode 100644 motorApp/Db/motorStatus.db diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index f872ed32b..2739dc11d 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -37,6 +37,7 @@ DB += ACRAuxBo.template DB += ACRAuxBoRBV.template DB += TransPos.db DB += motorUtil.db +DB += motorStatus.db DB += asyn_motor.db DB += profileMoveController.template DB += profileMoveAxis.template diff --git a/motorApp/Db/motorStatus.db b/motorApp/Db/motorStatus.db new file mode 100644 index 000000000..6ef2e4aea --- /dev/null +++ b/motorApp/Db/motorStatus.db @@ -0,0 +1,32 @@ +# monitor the motor status and update a string field with results +record(aSub, "$(P)$(M):_MSTACALC") +{ + field(SNAM, "displayMotorMSTA") + field(INPA, "$(P)$(M).MSTA CP") + field(FTA, "ULONG") + field(OUTA, "$(P)$(M)_MSTA_STR PP") + field(FTVA, "CHAR") + field(NOVA, "512") +} + +record(waveform, "$(P)$(M)_MSTA_STR") +{ + field(NELM, "512") + field(FTVL, "CHAR") + field(DESC, "Text version of motor MSTA") +} + +record(aSub, "$(P)$(M):_STATUSCALC") +{ + field(SNAM, "displayMotorMSTASimple") + field(INPA, "$(P)$(M).MSTA CP") + field(FTA, "ULONG") + field(OUTA, "$(P)$(M)_STATUS PP") + field(FTVA, "STRING") + field(NOVA, "1") +} + +record(stringin, "$(P)$(M)_STATUS") +{ + field(DESC, "Simple Text version of motor MSTA") +} \ No newline at end of file From a6f4a209dbab4e5d5d856cac6867399212b7b418 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 17 Aug 2016 16:01:54 +0100 Subject: [PATCH 029/232] Add ERES to default record properties. Search shows only used by McLennan --- motorApp/Db/motor.db | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 667adebca..2a7bae038 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -15,6 +15,7 @@ record(motor, "$(P)$(M)") { field(BACC, "$(BACC)") field(OUT, "#C$(C) S$(S) @") field(MRES, "$(MRES)") + field(ERES, "$(ERES)") field(PREC, "$(PREC)") field(EGU, "$(EGU)") field(DHLM, "$(DHLM)") From d7d046f44e239e4b27335ff339764d07c37cc42b Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Fri, 19 Aug 2016 11:47:25 +0100 Subject: [PATCH 030/232] Create specialised motor record for McLennan --- motorApp/Db/asyn_motor_plus_eres.db | 96 +++++++++++++++++++++++++++++ motorApp/Db/motor.db | 1 - 2 files changed, 96 insertions(+), 1 deletion(-) create mode 100644 motorApp/Db/asyn_motor_plus_eres.db diff --git a/motorApp/Db/asyn_motor_plus_eres.db b/motorApp/Db/asyn_motor_plus_eres.db new file mode 100644 index 000000000..3bbb59730 --- /dev/null +++ b/motorApp/Db/asyn_motor_plus_eres.db @@ -0,0 +1,96 @@ +#! Generated by VisualDCT v2.6 +#! DBDSTART +#! DBDEND + + +record(motor, "$(P)$(M)") { + field(DESC, "$(DESC)") + field(DTYP, "$(DTYP)") + field(DIR, "$(DIR)") + field(VELO, "$(VELO)") + field(VBAS, "$(VBAS)") + field(ACCL, "$(ACCL)") + field(BDST, "$(BDST)") + field(BVEL, "$(BVEL)") + field(BACC, "$(BACC)") + field(OUT, "@asyn($(PORT),$(ADDR))") + field(MRES, "$(MRES)") + field(ERES, "$(ERES)") + field(PREC, "$(PREC)") + field(EGU, "$(EGU)") + field(DHLM, "$(DHLM)") + field(DLLM, "$(DLLM)") + field(INIT, "$(INIT)") + field(RTRY, "$(RTRY=10)") + field(TWV, "1") + field(SDIS, "$(P)$(M)_able.VAL") +} + +# ISIS local aliases for genie_python +alias("$(P)$(M)", "$(P)$(M):SP") +alias("$(P)$(M)", "$(P)$(M):SP:RBV") + +record(bo, "$(P)$(M)_able") { + field(DESC, "motor enable") + field(PINI, "YES") + field(OUT, "$(P)$(M).DISP") + field(ZNAM, "Enable") + field(ONAM, "Disable") +} + +record(calcout, "$(P)$(M)_vCh") { + field(DESC, "change velocity") + field(CALC, "min(max(a*b,c),d)") + field(INPB, "$(P)$(M).S") + field(INPC, "$(P)$(M).SBAS") + field(INPD, "$(P)$(M).SMAX") + field(OUT, "$(P)$(M).S") +} + +record(calcout, "$(P)$(M)_twCh") { + field(DESC, "change TWV") + field(CALC, "min(max(a*b,c),d-e)") + field(INPB, "$(P)$(M).TWV") + field(INPC, "$(P)$(M).MRES") + field(INPD, "$(P)$(M).HLM") + field(INPE, "$(P)$(M).LLM") + field(OUT, "$(P)$(M).TWV") +} + +#! Further lines contain data used by VisualDCT +#! View(405,484,0.9) +#! Record("$(P)$(M)",840,675,0,0,"$(P)$(M)") +#! Field("$(P)$(M).DISP",16777215,1,"$(P)$(M).DISP") +#! Field("$(P)$(M).S",16777215,0,"$(P)$(M).S") +#! Field("$(P)$(M).SBAS",16777215,0,"$(P)$(M).SBAS") +#! Field("$(P)$(M).SMAX",16777215,0,"$(P)$(M).SMAX") +#! Field("$(P)$(M).TWV",16777215,0,"$(P)$(M).TWV") +#! Field("$(P)$(M).MRES",16777215,0,"$(P)$(M).MRES") +#! Field("$(P)$(M).HLM",16777215,0,"$(P)$(M).HLM") +#! Field("$(P)$(M).LLM",16777215,0,"$(P)$(M).LLM") +#! Field("$(P)$(M).SDIS",16777215,1,"$(P)$(M).SDIS") +#! Link("$(P)$(M).SDIS","$(P)$(M)_able.VAL") +#! Record("$(P)$(M)_able",1120,874,0,0,"$(P)$(M)_able") +#! Field("$(P)$(M)_able.OUT",16777215,0,"$(P)$(M)_able.OUT") +#! Link("$(P)$(M)_able.OUT","$(P)$(M).DISP") +#! Field("$(P)$(M)_able.VAL",16777215,0,"$(P)$(M)_able.VAL") +#! Record("$(P)$(M)_vCh",440,699,0,0,"$(P)$(M)_vCh") +#! Field("$(P)$(M)_vCh.OUT",16777215,1,"$(P)$(M)_vCh.OUT") +#! Link("$(P)$(M)_vCh.OUT","$(P)$(M).S") +#! Field("$(P)$(M)_vCh.INPB",16777215,1,"$(P)$(M)_vCh.INPB") +#! Link("$(P)$(M)_vCh.INPB","$(P)$(M).S") +#! Field("$(P)$(M)_vCh.INPC",16777215,1,"$(P)$(M)_vCh.INPC") +#! Link("$(P)$(M)_vCh.INPC","$(P)$(M).SBAS") +#! Field("$(P)$(M)_vCh.INPD",16777215,1,"$(P)$(M)_vCh.INPD") +#! Link("$(P)$(M)_vCh.INPD","$(P)$(M).SMAX") +#! Record("$(P)$(M)_twCh",440,925,0,0,"$(P)$(M)_twCh") +#! Field("$(P)$(M)_twCh.OUT",16777215,1,"$(P)$(M)_twCh.OUT") +#! Link("$(P)$(M)_twCh.OUT","$(P)$(M).TWV") +#! Field("$(P)$(M)_twCh.INPB",16777215,1,"$(P)$(M)_twCh.INPB") +#! Link("$(P)$(M)_twCh.INPB","$(P)$(M).TWV") +#! Field("$(P)$(M)_twCh.INPC",16777215,1,"$(P)$(M)_twCh.INPC") +#! Link("$(P)$(M)_twCh.INPC","$(P)$(M).MRES") +#! Field("$(P)$(M)_twCh.INPD",16777215,1,"$(P)$(M)_twCh.INPD") +#! Link("$(P)$(M)_twCh.INPD","$(P)$(M).HLM") +#! Field("$(P)$(M)_twCh.INPE",16777215,1,"$(P)$(M)_twCh.INPE") +#! Link("$(P)$(M)_twCh.INPE","$(P)$(M).LLM") diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 2a7bae038..667adebca 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -15,7 +15,6 @@ record(motor, "$(P)$(M)") { field(BACC, "$(BACC)") field(OUT, "#C$(C) S$(S) @") field(MRES, "$(MRES)") - field(ERES, "$(ERES)") field(PREC, "$(PREC)") field(EGU, "$(EGU)") field(DHLM, "$(DHLM)") From 8ea2265aff66b22583634e6b9ba65d83d6d3017e Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Fri, 19 Aug 2016 16:13:43 +0100 Subject: [PATCH 031/232] Go back to using normal motor record --- motorApp/Db/asyn_motor_plus_eres.db | 96 ----------------------------- motorApp/Db/motor.db | 1 + 2 files changed, 1 insertion(+), 96 deletions(-) delete mode 100644 motorApp/Db/asyn_motor_plus_eres.db diff --git a/motorApp/Db/asyn_motor_plus_eres.db b/motorApp/Db/asyn_motor_plus_eres.db deleted file mode 100644 index 3bbb59730..000000000 --- a/motorApp/Db/asyn_motor_plus_eres.db +++ /dev/null @@ -1,96 +0,0 @@ -#! Generated by VisualDCT v2.6 -#! DBDSTART -#! DBDEND - - -record(motor, "$(P)$(M)") { - field(DESC, "$(DESC)") - field(DTYP, "$(DTYP)") - field(DIR, "$(DIR)") - field(VELO, "$(VELO)") - field(VBAS, "$(VBAS)") - field(ACCL, "$(ACCL)") - field(BDST, "$(BDST)") - field(BVEL, "$(BVEL)") - field(BACC, "$(BACC)") - field(OUT, "@asyn($(PORT),$(ADDR))") - field(MRES, "$(MRES)") - field(ERES, "$(ERES)") - field(PREC, "$(PREC)") - field(EGU, "$(EGU)") - field(DHLM, "$(DHLM)") - field(DLLM, "$(DLLM)") - field(INIT, "$(INIT)") - field(RTRY, "$(RTRY=10)") - field(TWV, "1") - field(SDIS, "$(P)$(M)_able.VAL") -} - -# ISIS local aliases for genie_python -alias("$(P)$(M)", "$(P)$(M):SP") -alias("$(P)$(M)", "$(P)$(M):SP:RBV") - -record(bo, "$(P)$(M)_able") { - field(DESC, "motor enable") - field(PINI, "YES") - field(OUT, "$(P)$(M).DISP") - field(ZNAM, "Enable") - field(ONAM, "Disable") -} - -record(calcout, "$(P)$(M)_vCh") { - field(DESC, "change velocity") - field(CALC, "min(max(a*b,c),d)") - field(INPB, "$(P)$(M).S") - field(INPC, "$(P)$(M).SBAS") - field(INPD, "$(P)$(M).SMAX") - field(OUT, "$(P)$(M).S") -} - -record(calcout, "$(P)$(M)_twCh") { - field(DESC, "change TWV") - field(CALC, "min(max(a*b,c),d-e)") - field(INPB, "$(P)$(M).TWV") - field(INPC, "$(P)$(M).MRES") - field(INPD, "$(P)$(M).HLM") - field(INPE, "$(P)$(M).LLM") - field(OUT, "$(P)$(M).TWV") -} - -#! Further lines contain data used by VisualDCT -#! View(405,484,0.9) -#! Record("$(P)$(M)",840,675,0,0,"$(P)$(M)") -#! Field("$(P)$(M).DISP",16777215,1,"$(P)$(M).DISP") -#! Field("$(P)$(M).S",16777215,0,"$(P)$(M).S") -#! Field("$(P)$(M).SBAS",16777215,0,"$(P)$(M).SBAS") -#! Field("$(P)$(M).SMAX",16777215,0,"$(P)$(M).SMAX") -#! Field("$(P)$(M).TWV",16777215,0,"$(P)$(M).TWV") -#! Field("$(P)$(M).MRES",16777215,0,"$(P)$(M).MRES") -#! Field("$(P)$(M).HLM",16777215,0,"$(P)$(M).HLM") -#! Field("$(P)$(M).LLM",16777215,0,"$(P)$(M).LLM") -#! Field("$(P)$(M).SDIS",16777215,1,"$(P)$(M).SDIS") -#! Link("$(P)$(M).SDIS","$(P)$(M)_able.VAL") -#! Record("$(P)$(M)_able",1120,874,0,0,"$(P)$(M)_able") -#! Field("$(P)$(M)_able.OUT",16777215,0,"$(P)$(M)_able.OUT") -#! Link("$(P)$(M)_able.OUT","$(P)$(M).DISP") -#! Field("$(P)$(M)_able.VAL",16777215,0,"$(P)$(M)_able.VAL") -#! Record("$(P)$(M)_vCh",440,699,0,0,"$(P)$(M)_vCh") -#! Field("$(P)$(M)_vCh.OUT",16777215,1,"$(P)$(M)_vCh.OUT") -#! Link("$(P)$(M)_vCh.OUT","$(P)$(M).S") -#! Field("$(P)$(M)_vCh.INPB",16777215,1,"$(P)$(M)_vCh.INPB") -#! Link("$(P)$(M)_vCh.INPB","$(P)$(M).S") -#! Field("$(P)$(M)_vCh.INPC",16777215,1,"$(P)$(M)_vCh.INPC") -#! Link("$(P)$(M)_vCh.INPC","$(P)$(M).SBAS") -#! Field("$(P)$(M)_vCh.INPD",16777215,1,"$(P)$(M)_vCh.INPD") -#! Link("$(P)$(M)_vCh.INPD","$(P)$(M).SMAX") -#! Record("$(P)$(M)_twCh",440,925,0,0,"$(P)$(M)_twCh") -#! Field("$(P)$(M)_twCh.OUT",16777215,1,"$(P)$(M)_twCh.OUT") -#! Link("$(P)$(M)_twCh.OUT","$(P)$(M).TWV") -#! Field("$(P)$(M)_twCh.INPB",16777215,1,"$(P)$(M)_twCh.INPB") -#! Link("$(P)$(M)_twCh.INPB","$(P)$(M).TWV") -#! Field("$(P)$(M)_twCh.INPC",16777215,1,"$(P)$(M)_twCh.INPC") -#! Link("$(P)$(M)_twCh.INPC","$(P)$(M).MRES") -#! Field("$(P)$(M)_twCh.INPD",16777215,1,"$(P)$(M)_twCh.INPD") -#! Link("$(P)$(M)_twCh.INPD","$(P)$(M).HLM") -#! Field("$(P)$(M)_twCh.INPE",16777215,1,"$(P)$(M)_twCh.INPE") -#! Link("$(P)$(M)_twCh.INPE","$(P)$(M).LLM") diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 667adebca..11080a2bb 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -15,6 +15,7 @@ record(motor, "$(P)$(M)") { field(BACC, "$(BACC)") field(OUT, "#C$(C) S$(S) @") field(MRES, "$(MRES)") + field(MRES, "$(ERES)") field(PREC, "$(PREC)") field(EGU, "$(EGU)") field(DHLM, "$(DHLM)") From 01c12966907673fbb04ccea4e23fa88566b73c9d Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Fri, 19 Aug 2016 16:28:51 +0100 Subject: [PATCH 032/232] A bit of an important typo --- motorApp/Db/motor.db | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 11080a2bb..2a7bae038 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -15,7 +15,7 @@ record(motor, "$(P)$(M)") { field(BACC, "$(BACC)") field(OUT, "#C$(C) S$(S) @") field(MRES, "$(MRES)") - field(MRES, "$(ERES)") + field(ERES, "$(ERES)") field(PREC, "$(PREC)") field(EGU, "$(EGU)") field(DHLM, "$(DHLM)") From 22d0a96a5526c4eecad46c33d4d156ec428086a2 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 23 Aug 2016 09:52:06 +0100 Subject: [PATCH 033/232] Let McLennan have its own DB file rather than modifying the base type --- motorApp/Db/mclennan_motor.db | 95 +++++++++++++++++++++++++++++++++++ motorApp/Db/motor.db | 1 - 2 files changed, 95 insertions(+), 1 deletion(-) create mode 100644 motorApp/Db/mclennan_motor.db diff --git a/motorApp/Db/mclennan_motor.db b/motorApp/Db/mclennan_motor.db new file mode 100644 index 000000000..6726a534f --- /dev/null +++ b/motorApp/Db/mclennan_motor.db @@ -0,0 +1,95 @@ +#! Generated by VisualDCT v2.6 +#! DBDSTART +#! DBDEND + + +record(motor, "$(P)$(M)") { + field(DESC, "$(DESC)") + field(DTYP, "$(DTYP)") + field(DIR, "$(DIR)") + field(VELO, "$(VELO)") + field(VBAS, "$(VBAS)") + field(ACCL, "$(ACCL)") + field(BDST, "$(BDST)") + field(BVEL, "$(BVEL)") + field(BACC, "$(BACC)") + field(OUT, "$(OUT)") + field(MRES, "$(MRES)") + field(ERES, "$(ERES)") + field(PREC, "$(PREC)") + field(EGU, "$(EGU)") + field(DHLM, "$(DHLM)") + field(DLLM, "$(DLLM)") + field(INIT, "$(INIT)") + field(TWV, "1") + field(SDIS, "$(P)$(M)_able.VAL") +} + +# ISIS local aliases for genie_python +alias("$(P)$(M)", "$(P)$(M):SP") +alias("$(P)$(M)", "$(P)$(M):SP:RBV") + +record(bo, "$(P)$(M)_able") { + field(DESC, "motor enable") + field(PINI, "YES") + field(OUT, "$(P)$(M).DISP") + field(ZNAM, "Enable") + field(ONAM, "Disable") +} + +record(calcout, "$(P)$(M)_vCh") { + field(DESC, "change velocity") + field(CALC, "min(max(a*b,c),d)") + field(INPB, "$(P)$(M).S") + field(INPC, "$(P)$(M).SBAS") + field(INPD, "$(P)$(M).SMAX") + field(OUT, "$(P)$(M).S") +} + +record(calcout, "$(P)$(M)_twCh") { + field(DESC, "change TWV") + field(CALC, "min(max(a*b,c),d-e)") + field(INPB, "$(P)$(M).TWV") + field(INPC, "$(P)$(M).MRES") + field(INPD, "$(P)$(M).HLM") + field(INPE, "$(P)$(M).LLM") + field(OUT, "$(P)$(M).TWV") +} + +#! Further lines contain data used by VisualDCT +#! View(405,680,1.0) +#! Record("$(P)$(M)",840,674,0,0,"$(P)$(M)") +#! Field("$(P)$(M).DISP",16777215,1,"$(P)$(M).DISP") +#! Field("$(P)$(M).S",16777215,0,"$(P)$(M).S") +#! Field("$(P)$(M).SBAS",16777215,0,"$(P)$(M).SBAS") +#! Field("$(P)$(M).SMAX",16777215,0,"$(P)$(M).SMAX") +#! Field("$(P)$(M).TWV",16777215,0,"$(P)$(M).TWV") +#! Field("$(P)$(M).MRES",16777215,0,"$(P)$(M).MRES") +#! Field("$(P)$(M).HLM",16777215,0,"$(P)$(M).HLM") +#! Field("$(P)$(M).LLM",16777215,0,"$(P)$(M).LLM") +#! Field("$(P)$(M).SDIS",16777215,1,"$(P)$(M).SDIS") +#! Link("$(P)$(M).SDIS","$(P)$(M)_able.VAL") +#! Record("$(P)$(M)_able",1140,854,0,0,"$(P)$(M)_able") +#! Field("$(P)$(M)_able.OUT",16777215,0,"$(P)$(M)_able.OUT") +#! Link("$(P)$(M)_able.OUT","$(P)$(M).DISP") +#! Field("$(P)$(M)_able.VAL",16777215,0,"$(P)$(M)_able.VAL") +#! Record("$(P)$(M)_vCh",440,700,0,0,"$(P)$(M)_vCh") +#! Field("$(P)$(M)_vCh.OUT",16777215,1,"$(P)$(M)_vCh.OUT") +#! Link("$(P)$(M)_vCh.OUT","$(P)$(M).S") +#! Field("$(P)$(M)_vCh.INPB",16777215,1,"$(P)$(M)_vCh.INPB") +#! Link("$(P)$(M)_vCh.INPB","$(P)$(M).S") +#! Field("$(P)$(M)_vCh.INPC",16777215,1,"$(P)$(M)_vCh.INPC") +#! Link("$(P)$(M)_vCh.INPC","$(P)$(M).SBAS") +#! Field("$(P)$(M)_vCh.INPD",16777215,1,"$(P)$(M)_vCh.INPD") +#! Link("$(P)$(M)_vCh.INPD","$(P)$(M).SMAX") +#! Record("$(P)$(M)_twCh",440,926,0,0,"$(P)$(M)_twCh") +#! Field("$(P)$(M)_twCh.OUT",16777215,1,"$(P)$(M)_twCh.OUT") +#! Link("$(P)$(M)_twCh.OUT","$(P)$(M).TWV") +#! Field("$(P)$(M)_twCh.INPB",16777215,1,"$(P)$(M)_twCh.INPB") +#! Link("$(P)$(M)_twCh.INPB","$(P)$(M).TWV") +#! Field("$(P)$(M)_twCh.INPC",16777215,1,"$(P)$(M)_twCh.INPC") +#! Link("$(P)$(M)_twCh.INPC","$(P)$(M).MRES") +#! Field("$(P)$(M)_twCh.INPD",16777215,1,"$(P)$(M)_twCh.INPD") +#! Link("$(P)$(M)_twCh.INPD","$(P)$(M).HLM") +#! Field("$(P)$(M)_twCh.INPE",16777215,1,"$(P)$(M)_twCh.INPE") +#! Link("$(P)$(M)_twCh.INPE","$(P)$(M).LLM") diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 2a7bae038..667adebca 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -15,7 +15,6 @@ record(motor, "$(P)$(M)") { field(BACC, "$(BACC)") field(OUT, "#C$(C) S$(S) @") field(MRES, "$(MRES)") - field(ERES, "$(ERES)") field(PREC, "$(PREC)") field(EGU, "$(EGU)") field(DHLM, "$(DHLM)") From f8e791f54362be1df2459611f3ffe8f0bac7b710 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 1 Sep 2016 19:17:08 +0100 Subject: [PATCH 034/232] Move additions to standard motor.db Option ERES etc. is more generic than just McLennan --- motorApp/Db/mclennan_motor.db | 95 ----------------------------------- motorApp/Db/motor.db | 4 +- 2 files changed, 3 insertions(+), 96 deletions(-) delete mode 100644 motorApp/Db/mclennan_motor.db diff --git a/motorApp/Db/mclennan_motor.db b/motorApp/Db/mclennan_motor.db deleted file mode 100644 index 6726a534f..000000000 --- a/motorApp/Db/mclennan_motor.db +++ /dev/null @@ -1,95 +0,0 @@ -#! Generated by VisualDCT v2.6 -#! DBDSTART -#! DBDEND - - -record(motor, "$(P)$(M)") { - field(DESC, "$(DESC)") - field(DTYP, "$(DTYP)") - field(DIR, "$(DIR)") - field(VELO, "$(VELO)") - field(VBAS, "$(VBAS)") - field(ACCL, "$(ACCL)") - field(BDST, "$(BDST)") - field(BVEL, "$(BVEL)") - field(BACC, "$(BACC)") - field(OUT, "$(OUT)") - field(MRES, "$(MRES)") - field(ERES, "$(ERES)") - field(PREC, "$(PREC)") - field(EGU, "$(EGU)") - field(DHLM, "$(DHLM)") - field(DLLM, "$(DLLM)") - field(INIT, "$(INIT)") - field(TWV, "1") - field(SDIS, "$(P)$(M)_able.VAL") -} - -# ISIS local aliases for genie_python -alias("$(P)$(M)", "$(P)$(M):SP") -alias("$(P)$(M)", "$(P)$(M):SP:RBV") - -record(bo, "$(P)$(M)_able") { - field(DESC, "motor enable") - field(PINI, "YES") - field(OUT, "$(P)$(M).DISP") - field(ZNAM, "Enable") - field(ONAM, "Disable") -} - -record(calcout, "$(P)$(M)_vCh") { - field(DESC, "change velocity") - field(CALC, "min(max(a*b,c),d)") - field(INPB, "$(P)$(M).S") - field(INPC, "$(P)$(M).SBAS") - field(INPD, "$(P)$(M).SMAX") - field(OUT, "$(P)$(M).S") -} - -record(calcout, "$(P)$(M)_twCh") { - field(DESC, "change TWV") - field(CALC, "min(max(a*b,c),d-e)") - field(INPB, "$(P)$(M).TWV") - field(INPC, "$(P)$(M).MRES") - field(INPD, "$(P)$(M).HLM") - field(INPE, "$(P)$(M).LLM") - field(OUT, "$(P)$(M).TWV") -} - -#! Further lines contain data used by VisualDCT -#! View(405,680,1.0) -#! Record("$(P)$(M)",840,674,0,0,"$(P)$(M)") -#! Field("$(P)$(M).DISP",16777215,1,"$(P)$(M).DISP") -#! Field("$(P)$(M).S",16777215,0,"$(P)$(M).S") -#! Field("$(P)$(M).SBAS",16777215,0,"$(P)$(M).SBAS") -#! Field("$(P)$(M).SMAX",16777215,0,"$(P)$(M).SMAX") -#! Field("$(P)$(M).TWV",16777215,0,"$(P)$(M).TWV") -#! Field("$(P)$(M).MRES",16777215,0,"$(P)$(M).MRES") -#! Field("$(P)$(M).HLM",16777215,0,"$(P)$(M).HLM") -#! Field("$(P)$(M).LLM",16777215,0,"$(P)$(M).LLM") -#! Field("$(P)$(M).SDIS",16777215,1,"$(P)$(M).SDIS") -#! Link("$(P)$(M).SDIS","$(P)$(M)_able.VAL") -#! Record("$(P)$(M)_able",1140,854,0,0,"$(P)$(M)_able") -#! Field("$(P)$(M)_able.OUT",16777215,0,"$(P)$(M)_able.OUT") -#! Link("$(P)$(M)_able.OUT","$(P)$(M).DISP") -#! Field("$(P)$(M)_able.VAL",16777215,0,"$(P)$(M)_able.VAL") -#! Record("$(P)$(M)_vCh",440,700,0,0,"$(P)$(M)_vCh") -#! Field("$(P)$(M)_vCh.OUT",16777215,1,"$(P)$(M)_vCh.OUT") -#! Link("$(P)$(M)_vCh.OUT","$(P)$(M).S") -#! Field("$(P)$(M)_vCh.INPB",16777215,1,"$(P)$(M)_vCh.INPB") -#! Link("$(P)$(M)_vCh.INPB","$(P)$(M).S") -#! Field("$(P)$(M)_vCh.INPC",16777215,1,"$(P)$(M)_vCh.INPC") -#! Link("$(P)$(M)_vCh.INPC","$(P)$(M).SBAS") -#! Field("$(P)$(M)_vCh.INPD",16777215,1,"$(P)$(M)_vCh.INPD") -#! Link("$(P)$(M)_vCh.INPD","$(P)$(M).SMAX") -#! Record("$(P)$(M)_twCh",440,926,0,0,"$(P)$(M)_twCh") -#! Field("$(P)$(M)_twCh.OUT",16777215,1,"$(P)$(M)_twCh.OUT") -#! Link("$(P)$(M)_twCh.OUT","$(P)$(M).TWV") -#! Field("$(P)$(M)_twCh.INPB",16777215,1,"$(P)$(M)_twCh.INPB") -#! Link("$(P)$(M)_twCh.INPB","$(P)$(M).TWV") -#! Field("$(P)$(M)_twCh.INPC",16777215,1,"$(P)$(M)_twCh.INPC") -#! Link("$(P)$(M)_twCh.INPC","$(P)$(M).MRES") -#! Field("$(P)$(M)_twCh.INPD",16777215,1,"$(P)$(M)_twCh.INPD") -#! Link("$(P)$(M)_twCh.INPD","$(P)$(M).HLM") -#! Field("$(P)$(M)_twCh.INPE",16777215,1,"$(P)$(M)_twCh.INPE") -#! Link("$(P)$(M)_twCh.INPE","$(P)$(M).LLM") diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 667adebca..429cbc9dd 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -13,8 +13,10 @@ record(motor, "$(P)$(M)") { field(BDST, "$(BDST)") field(BVEL, "$(BVEL)") field(BACC, "$(BACC)") - field(OUT, "#C$(C) S$(S) @") + field(OUT, "#C$(C) S$(S) @$(MOTORSIM=)") field(MRES, "$(MRES)") + field(ERES, "$(ERES=)") + field(UEIP, "$(UEIP=0)") field(PREC, "$(PREC)") field(EGU, "$(EGU)") field(DHLM, "$(DHLM)") From 7e9e29a1808a4e8db1a155f3626ff0fb8b5528f4 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Mon, 24 Oct 2016 10:23:20 +0100 Subject: [PATCH 035/232] "Adding .project files to git ignore" --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index e476e7aeb..2168c4eb8 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,4 @@ runIOC.bat runIOC.sh relPaths.sh iocBoot/iocmotorSim/motor.substitutions.local +.project From 96ae31afe38287bc06c2d168f2466b295fee2099 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Fri, 4 Nov 2016 11:52:41 +0000 Subject: [PATCH 036/232] Use the CV command for homing with the previously set velocity --- motorApp/MclennanSrc/devPM304.cc | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 2cfb2e5df..92c14f439 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -66,6 +66,7 @@ STATIC long PM304_init_record(void *); STATIC long PM304_start_trans(struct motorRecord *); STATIC RTN_STATUS PM304_build_trans(motor_cmnd, double *, struct motorRecord *); STATIC RTN_STATUS PM304_end_trans(struct motorRecord *); +STATIC long VELO; struct motor_dset devPM304 = { @@ -232,14 +233,18 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo if (cntrl->model == MODEL_PM304){ sprintf(buff, "%dIX;", axis); } else { - sprintf(buff, "%dHD;", axis); + //sprintf(buff, "%dHD;", axis); + // Motor always sends an SV before CV for VELO will be current base velocity + sprintf(buff, "%dCV%ld;", axis, VELO); } break; case HOME_REV: if (cntrl->model == MODEL_PM304){ sprintf(buff, "%dIX-1;", axis); } else { - sprintf(buff, "%dHD-1;", axis); + //sprintf(buff, "%dHD-1;", axis); + // Motor always sends an SV before CV for VELO will be current base velocity + sprintf(buff, "%dCV-%ld;", axis, VELO); } break; case LOAD_POS: @@ -253,10 +258,10 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo break; /* PM304 does not use base velocity */ case SET_VELOCITY: sprintf(buff, "%dSV%ld;", axis, ival); + VELO = ival; break; case SET_ACCEL: sprintf(buff, "%dSA%ld;", axis, ival); - strcat(motor_call->message, buff); sprintf(buff, "%dSD%ld;", axis, ival); break; case GO: From b59342e8cade126310f6f18b50df8d17c6de3572 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Fri, 4 Nov 2016 11:53:24 +0000 Subject: [PATCH 037/232] Set VELO initially 0 just to be safe --- motorApp/MclennanSrc/devPM304.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 92c14f439..0d00c7b6f 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -66,7 +66,7 @@ STATIC long PM304_init_record(void *); STATIC long PM304_start_trans(struct motorRecord *); STATIC RTN_STATUS PM304_build_trans(motor_cmnd, double *, struct motorRecord *); STATIC RTN_STATUS PM304_end_trans(struct motorRecord *); -STATIC long VELO; +STATIC long VELO = 0; struct motor_dset devPM304 = { From a85a54a442274ccdc30f972a6b4118c961fdae96 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Fri, 4 Nov 2016 12:04:34 +0000 Subject: [PATCH 038/232] Tab to space --- motorApp/MclennanSrc/devPM304.cc | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 0d00c7b6f..18e2b909d 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -16,7 +16,7 @@ * motorRecord * .03 02-11-03 mlr Version 3.0, added support for PM600 model. * Added SD for decceleration. - * .04 05/27/03 rls R3.14 conversion. + * .04 05/27/03 rls R3.14 conversion. * ... */ @@ -24,12 +24,12 @@ #define VERSION 3.00 -#include +#include #include "motorRecord.h" #include "motor.h" #include "motordevCom.h" #include "drvPM304.h" -#include "epicsExport.h" +#include "epicsExport.h" #define STATIC static @@ -121,7 +121,7 @@ STATIC long PM304_init(void *arg) if (after == 0) { - drvtabptr = &PM304_access; + drvtabptr = &PM304_access; (drvtabptr->init)(); } @@ -234,8 +234,8 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dIX;", axis); } else { //sprintf(buff, "%dHD;", axis); - // Motor always sends an SV before CV for VELO will be current base velocity - sprintf(buff, "%dCV%ld;", axis, VELO); + // Motor always sends an SV before CV for VELO will be current base velocity + sprintf(buff, "%dCV%ld;", axis, VELO); } break; case HOME_REV: @@ -243,8 +243,8 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dIX-1;", axis); } else { //sprintf(buff, "%dHD-1;", axis); - // Motor always sends an SV before CV for VELO will be current base velocity - sprintf(buff, "%dCV-%ld;", axis, VELO); + // Motor always sends an SV before CV for VELO will be current base velocity + sprintf(buff, "%dCV-%ld;", axis, VELO); } break; case LOAD_POS: @@ -258,7 +258,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo break; /* PM304 does not use base velocity */ case SET_VELOCITY: sprintf(buff, "%dSV%ld;", axis, ival); - VELO = ival; + VELO = ival; break; case SET_ACCEL: sprintf(buff, "%dSA%ld;", axis, ival); From 0537999ee5a189cd788d1676d571ea791392f593 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Fri, 4 Nov 2016 12:05:26 +0000 Subject: [PATCH 039/232] Revert change to SET_ACCEL --- motorApp/MclennanSrc/devPM304.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 18e2b909d..fa66f5a86 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -262,6 +262,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo break; case SET_ACCEL: sprintf(buff, "%dSA%ld;", axis, ival); + strcat(motor_call->message, buff); sprintf(buff, "%dSD%ld;", axis, ival); break; case GO: From 1b4de4392694c5d4e83e3953e6d57c74a5f8bcf3 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 15 Nov 2016 13:43:36 +0000 Subject: [PATCH 040/232] Set up shell sequencer for mclennan homing --- motorApp/MclennanSrc/Makefile | 4 + motorApp/MclennanSrc/homing.dbd | 1 + motorApp/MclennanSrc/homing.st | 45 ++++++ motorApp/MclennanSrc/seqPVmacros.h | 245 +++++++++++++++++++++++++++++ 4 files changed, 295 insertions(+) create mode 100644 motorApp/MclennanSrc/homing.dbd create mode 100644 motorApp/MclennanSrc/homing.st create mode 100644 motorApp/MclennanSrc/seqPVmacros.h diff --git a/motorApp/MclennanSrc/Makefile b/motorApp/MclennanSrc/Makefile index 5cc645837..8531cf5fe 100644 --- a/motorApp/MclennanSrc/Makefile +++ b/motorApp/MclennanSrc/Makefile @@ -7,15 +7,19 @@ include $(TOP)/configure/CONFIG USR_CXXFLAGS += -DDEBUG DBD += devMclennanMotor.dbd +DBD += homing.dbd LIBRARY_IOC = Mclennan # Intelligent Motion Systems driver support. SRCS += MclennanRegister.cc +SRCS += homing.st SRCS += devPM304.cc drvPM304.cc Mclennan_LIBS += motor Mclennan_LIBS += asyn +Mclennan_LIBS += seq +Mclennan_LIBS += pv Mclennan_LIBS += $(EPICS_BASE_IOC_LIBS) include $(TOP)/configure/RULES diff --git a/motorApp/MclennanSrc/homing.dbd b/motorApp/MclennanSrc/homing.dbd new file mode 100644 index 000000000..160f10e8c --- /dev/null +++ b/motorApp/MclennanSrc/homing.dbd @@ -0,0 +1 @@ +registrar(homingRegistrar) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st new file mode 100644 index 000000000..a8fd85b23 --- /dev/null +++ b/motorApp/MclennanSrc/homing.st @@ -0,0 +1,45 @@ +program homing("MOTPV=xxx") + +#include "seqPVmacros.h" + +char* SNLtaskName; +int debug_flag; + +/* Turn on run-time debug messages */ +option +d; + +/* PV definitions */ +PV(int, homing_forward_pv, "{MOTPV}.HOMF", Monitor); +PV(int, homing_reverse_pv, "{MOTPV}.HOMR", Monitor); + +ss motor +{ + state ready + { + when (homing_reverse_pv==1) + { + /* Trigger reverse move */ + + DEBUG_PRINT(2, "init -> moving", 0, 0, 0, 0); + + } state moving + + when (homing_forward_pv==1) + { + /* Trigger forward move */ + + DEBUG_PRINT(2, "init -> moving", 0, 0, 0, 0); + + } state moving + } + + state moving + { + when (homing_forward_pv==0 && homing_reverse_pv==0) + { + + DEBUG_PRINT(2, "moving -> ready", 0, 0, 0, 0); + + } state ready + } +} diff --git a/motorApp/MclennanSrc/seqPVmacros.h b/motorApp/MclennanSrc/seqPVmacros.h new file mode 100644 index 000000000..33b8a1aa9 --- /dev/null +++ b/motorApp/MclennanSrc/seqPVmacros.h @@ -0,0 +1,245 @@ +/*================================================================ + * + * seqPVmacros.h -- PV-related macros for EPICS State Notation + * Language (SNL) code development + * + * 2004-May-19 [jemian] merged all DEBUG_PRINTn macros into one macro + * 2003-Oct-01 [jemian] changed errlogPrintf to printf (can't use in interrupts) + * 2003-Mar-20 [jemian] changed pvSet to PVPUT (and pvSetStr to PVPUTSTR) + * 2003-Mar-19 [jemian] changed printf to errlogPrintf + * 2002-Sep-24 [tischler] added pvSetStr and DEBUG_PRINT (a la Jemian) + * 2002-Jun-04 [lazmo] renamed to seqPVmacros.h + * 2002-Mar-08 [lazmo] added pvSet() + * 2002-Feb-21 [lazmo] fixed comments + * 2001-Nov-24 [lazmo] added PVA & PVAA + * 2001-Oct-18 [lazmo] original + * + * Author: + * M. Laznovsky -- lazmo_at_slac.stanford.edu + * + *================================================================ + */ + +/*---------------------------------------------------------------- + * PV() -- declare a variable and the EPICS PV it's assigned to, with + * optional monitor & event flag. One line takes the place of + * the usual: + * + * int blah; + * assign blah ... + * monitor blah; + * evflag blahFLag; + * sync blah ... + * + * Format: + * + * PV( varType, varName, pvName, other ) + * + * Where: + * + * varType is int, short, float, etc. + * varName is the variable name, e.g. foo + * pvName is the associated PV, e.g. "XYZ:ETC:FOO" + * other is one of the following: + * NoMon + * Monitor + * EvFlag + * + * Examples: + * + * PV (int, gui_goo, "{STN}:GUI:GOO", NoMon); <-- no monitor + * PV (float, gui_fudge, "{STN}:GUI:FUDGE", Monitor); <-- w/monitor + * PV (short, gui_run, "{STN}:GUI:RUN", EvFlag); <-- w/monitor & event flag + * + *---------------------------------------------------------------- + */ + +#define PV(_TYPE_,_VAR_,_PV_,_OTHER_) \ + _TYPE_ _VAR_; \ + assign _VAR_ to _PV_ \ + _OTHER_ (_VAR_) + +/*---------------------------------------------------------------- + * PVA(), for single waveform rec or array of PVs + * + * "varName" becomes array of ; 3rd arg is number of elements + * + * examples: + * + * single waveform record: + * + * PVA (short, plot_x0, 32, "{STN}:DATA:PLOT:X0", NoMon); + * + * array of PVs: + * + * #define PVA_zap { \ + * "{STN}:GUI:ZAP1", \ + * "{STN}:GUI:ZAP2", \ + * "{STN}:GUI:ZAP3" \ + * } + * PVA (int, zap, 3, PVA_zap, EvFlag); + * + *---------------------------------------------------------------- + */ + +#define PVA(_TYPE_,_VAR_,_NELEM_,_PV_,_OTHER_) \ + _TYPE_ _VAR_ [ _NELEM_ ]; \ + assign _VAR_ to _PV_ \ + _OTHER_ (_VAR_) + +/*---------------------------------------------------------------- + * PVAA(), for arrays of waveform records + * + * "varName" becomes double-dimensioned array of + * 3rd arg is number of waveform records + * 4th arg is number of elements per record + * Need to define another macro first for "_PV_" + * + * example: + * + * #define PVAA_plotx { \ + * "{STN}:DATA:PLOT:X1", \ + * "{STN}:DATA:PLOT:X2", \ + * "{STN}:DATA:PLOT:X3" \ + * } + * PVAA (short, plotx, 3, 500, PVAA_plotx, NoMon); + * + *---------------------------------------------------------------- + */ + +#define PVAA(_TYPE_,_VAR_,_NREC_,_NELEM_,_PV_,_OTHER_) \ + _TYPE_ _VAR_ [ _NREC_ ] [ _NELEM_ ]; \ + assign _VAR_ to _PV_ \ + _OTHER_ (_VAR_) + +/*---------------------------------------------------------------- + * macros for last arg of PV* ("_OTHER_") + *---------------------------------------------------------------- + */ + +/* + * no monitor + */ +#define NoMon(_VAR_) /* this macro intentionally left blank :P */ + +/* + * monitor + */ +#define Monitor(_VAR_) ; monitor _VAR_ + +/* + * monitor & event flag; flag var will be named "_EvFlag" + */ +#define EvFlag(_VAR_) \ + Monitor (_VAR_); \ + evflag _VAR_##_mon; \ + sync _VAR_ _VAR_##_mon + +/*================================================================*/ +/*================================================================*/ + +/*---------------------------------------------------------------- + * PVPUT() -- variable assign and pvPut() in one + * + * Format: + * + * PVPUT( varName, expr ) + * + * Where: + * + * varName is the variable name, e.g. foo + * expr is any C expression + * + * Examples: + * + * PVPUT (foo, 3); + * expands to: + * foo = 3; + * pvPut(foo); + * + * PVPUT (bar, xyz + 2); + * expands to: + * bar = xyz + 2; + * pvPut(bar); + * + *---------------------------------------------------------------- + */ + +#define PVPUT(_VAR_,_EXPR_) \ + { \ + _VAR_ = ( _EXPR_ ); \ + pvPut(_VAR_); \ + } + +/*---------------------------------------------------------------- + * PVPUTSTR() -- string assign and pvPut() in one + * + * Format: + * + * PVPUTSTR( strName, string expr ) + * + * Where: + * + * strName is the variable name, e.g. foo + * string expr is any C expression that points to a string + * + * Example: + * + * PVPUTSTR (foo, "error message"); + * expands to: + * strcpy(foo,"error message"); + * pvPut(foo); + * + *---------------------------------------------------------------- + */ + +#define PVPUTSTR(_STR_,_EXPR_) \ + { \ + strcpy(_STR_,_EXPR_); \ + pvPut(_STR_); \ + } + +/*================================================================*/ +/*================================================================*/ + +/*---------------------------------------------------------------- + * DEBUG_PRINT() -- print a debug string for a particular debug level + * + * Format: + * + * DEBUG_PRINT( debug_level, string format, v1, v2, v3, v4 ) + * + * Where: + * + * debug_level is the debugging level, ie 3 + * string format is an sprintf format specifier + * v1, v2, v3, v4 are optional arguments for the sprintf format + * + * Example: + * + * DEBUG_PRINT (1, "ERROR, how did I get here?", 0, 0, 0, 0 ); + * + *---------------------------------------------------------------- + */ + +#if defined(NO_DEBUGGING_OUTPUT) +#define DEBUG_PRINT(DEBUG_LEVEL, FMT,V1,V2,V3,V4) ; +#else + +#define DEBUG_PRINT(DEBUG_LEVEL,FMT,V1,V2,V3,V4) \ + if (debug_flag >= DEBUG_LEVEL) { \ + printf("<%s,%d,%s,%d> ", \ + __FILE__, __LINE__, \ + SNLtaskName, DEBUG_LEVEL); \ + printf(FMT,V1,V2,V3,V4); \ + printf("\n"); \ + } + + +#endif + +/*================================================================*/ +/*================================================================*/ +/*================================================================*/ + +/* end */ From c5f950daa6e9f886816da7e2939cf730c915f40d Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 15 Nov 2016 17:01:09 +0000 Subject: [PATCH 041/232] Make homing call jog, which uses constant velocity move --- motorApp/MclennanSrc/devPM304.cc | 25 +++++++++++----- motorApp/MclennanSrc/homing.st | 51 ++++++++++++++++++++++---------- 2 files changed, 52 insertions(+), 24 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index fa66f5a86..034ab7ce3 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -67,6 +67,7 @@ STATIC long PM304_start_trans(struct motorRecord *); STATIC RTN_STATUS PM304_build_trans(motor_cmnd, double *, struct motorRecord *); STATIC RTN_STATUS PM304_end_trans(struct motorRecord *); STATIC long VELO = 0; +STATIC int HOME_TYPE; struct motor_dset devPM304 = { @@ -241,18 +242,26 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo case HOME_REV: if (cntrl->model == MODEL_PM304){ sprintf(buff, "%dIX-1;", axis); - } else { - //sprintf(buff, "%dHD-1;", axis); - // Motor always sends an SV before CV for VELO will be current base velocity - sprintf(buff, "%dCV-%ld;", axis, VELO); - } + } + /* + * Homing for PM600 done via SNL. See homing.st + * else { + * //sprintf(buff, "%dHD-1;", axis); + * // Motor always sends an SV before CV for VELO will be current base velocity + * sprintf(buff, "%dCV-%ld;", axis, VELO); + * } + */ break; case LOAD_POS: if (cntrl->use_encoder[axis-1]){ sprintf(buff, "%dAP%ld;", axis, ival); - } else { - sprintf(buff, "%dCP%ld;", axis, ival); } + /* + * Homing for PM600 done via SNL. See homing.st + * else { + * sprintf(buff, "%dCP%ld;", axis, ival); + * } + */ break; case SET_VEL_BASE: break; /* PM304 does not use base velocity */ @@ -334,4 +343,4 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo Debug(3, "PM304_build_trans: buff=%s, motor_call->message=%s\n", buff, motor_call->message); return (rtnval); -} +} \ No newline at end of file diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index a8fd85b23..53c7c703b 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -1,8 +1,12 @@ -program homing("MOTPV=xxx") +program homing("MOTPV=xxx,MODE=0") #include "seqPVmacros.h" char* SNLtaskName; +int mode; +int jog_forward, jog_reverse; +assign jog_forward to "{MOTPV}.JOGF"; +assign jog_reverse to "{MOTPV}.JOGR"; int debug_flag; /* Turn on run-time debug messages */ @@ -11,35 +15,50 @@ option +d; /* PV definitions */ PV(int, homing_forward_pv, "{MOTPV}.HOMF", Monitor); PV(int, homing_reverse_pv, "{MOTPV}.HOMR", Monitor); +PV(int, movable, "{MOTPV}.DMOV", Monitor); ss motor { + state init + { + when () + { + /* Currently we only have one mode so it isn't used */ + mode = "{MODE}"; + } state ready + } + state ready { when (homing_reverse_pv==1) { - /* Trigger reverse move */ - - DEBUG_PRINT(2, "init -> moving", 0, 0, 0, 0); - - } state moving + } state reverse_home_requested when (homing_forward_pv==1) { - /* Trigger forward move */ - - DEBUG_PRINT(2, "init -> moving", 0, 0, 0, 0); - - } state moving + } state forward_home_requested + } + + state forward_home_requested + { + when (homing_forward_pv==0) { + jog_forward = 1; + pvPut(jog_forward); + } state moving + } + + state reverse_home_requested + { + when (homing_reverse_pv==0) { + jog_reverse = 1; + pvPut(jog_reverse); + } state moving } state moving - { - when (homing_forward_pv==0 && homing_reverse_pv==0) + { + when (movable==1) { - - DEBUG_PRINT(2, "moving -> ready", 0, 0, 0, 0); - } state ready } } From 55b796db65986c6ba8d06b35d901000feafb91d8 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 17 Nov 2016 11:41:53 +0000 Subject: [PATCH 042/232] Little bit of tidying up --- motorApp/MclennanSrc/devPM304.cc | 20 ++------------------ motorApp/MclennanSrc/homing.st | 32 ++++++++++++++++---------------- 2 files changed, 18 insertions(+), 34 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 034ab7ce3..a32ffdf83 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -233,35 +233,19 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo case HOME_FOR: if (cntrl->model == MODEL_PM304){ sprintf(buff, "%dIX;", axis); - } else { - //sprintf(buff, "%dHD;", axis); - // Motor always sends an SV before CV for VELO will be current base velocity - sprintf(buff, "%dCV%ld;", axis, VELO); } + /* Homing for PM600 done via SNL. See homing.st */ break; case HOME_REV: if (cntrl->model == MODEL_PM304){ sprintf(buff, "%dIX-1;", axis); } - /* - * Homing for PM600 done via SNL. See homing.st - * else { - * //sprintf(buff, "%dHD-1;", axis); - * // Motor always sends an SV before CV for VELO will be current base velocity - * sprintf(buff, "%dCV-%ld;", axis, VELO); - * } - */ + /* Homing for PM600 done via SNL. See homing.st */ break; case LOAD_POS: if (cntrl->use_encoder[axis-1]){ sprintf(buff, "%dAP%ld;", axis, ival); } - /* - * Homing for PM600 done via SNL. See homing.st - * else { - * sprintf(buff, "%dCP%ld;", axis, ival); - * } - */ break; case SET_VEL_BASE: break; /* PM304 does not use base velocity */ diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 53c7c703b..ccd2f6155 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -1,20 +1,20 @@ -program homing("MOTPV=xxx,MODE=0") +program homing("MOTPV=xxx,MODE=Constant_velocity") #include "seqPVmacros.h" char* SNLtaskName; -int mode; -int jog_forward, jog_reverse; -assign jog_forward to "{MOTPV}.JOGF"; -assign jog_reverse to "{MOTPV}.JOGR"; +char* mode; +int jog_forward_value, jog_reverse_value; +assign jog_forward_value to "{MOTPV}.JOGF"; +assign jog_reverse_value to "{MOTPV}.JOGR"; int debug_flag; /* Turn on run-time debug messages */ option +d; /* PV definitions */ -PV(int, homing_forward_pv, "{MOTPV}.HOMF", Monitor); -PV(int, homing_reverse_pv, "{MOTPV}.HOMR", Monitor); +PV(int, home_forward_pv, "{MOTPV}.HOMF", Monitor); +PV(int, home_reverse_pv, "{MOTPV}.HOMR", Monitor); PV(int, movable, "{MOTPV}.DMOV", Monitor); ss motor @@ -23,35 +23,35 @@ ss motor { when () { - /* Currently we only have one mode so it isn't used */ + /* Currently we only have one mode so it isn't used. */ mode = "{MODE}"; } state ready } state ready { - when (homing_reverse_pv==1) + when (home_reverse_pv==1) { } state reverse_home_requested - when (homing_forward_pv==1) + when (home_forward_pv==1) { } state forward_home_requested } state forward_home_requested { - when (homing_forward_pv==0) { - jog_forward = 1; - pvPut(jog_forward); + when (home_forward_pv==0) { + jog_forward_value = 1; + pvPut(jog_forward_value); } state moving } state reverse_home_requested { - when (homing_reverse_pv==0) { - jog_reverse = 1; - pvPut(jog_reverse); + when (home_reverse_pv==0) { + jog_reverse_value = 1; + pvPut(jog_reverse_value); } state moving } From 077b1eece410c18305bc533b86c826f281f2c4a0 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 17 Nov 2016 12:03:21 +0000 Subject: [PATCH 043/232] Remove unused variable --- motorApp/MclennanSrc/devPM304.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index a32ffdf83..4a32bf9c6 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -67,7 +67,6 @@ STATIC long PM304_start_trans(struct motorRecord *); STATIC RTN_STATUS PM304_build_trans(motor_cmnd, double *, struct motorRecord *); STATIC RTN_STATUS PM304_end_trans(struct motorRecord *); STATIC long VELO = 0; -STATIC int HOME_TYPE; struct motor_dset devPM304 = { From 355a001a6c56020c47452d44d070211797b99bd9 Mon Sep 17 00:00:00 2001 From: jonelmer Date: Wed, 7 Dec 2016 14:13:28 +0000 Subject: [PATCH 044/232] Hard limits now set by soft limit values. Changed overshoot behaviour to stop and overshoot, instead of overshooting then correcting. --- motorApp/MotorSimSrc/motorSimDriver.cpp | 30 ++++++++++++++++++++----- motorApp/MotorSimSrc/motorSimDriver.h | 3 +++ 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index cb3d2f23a..61319ae97 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -376,6 +376,24 @@ asynStatus motorSimAxis::poll(bool *moving) } +/** Set the high limit position of the motor. + * \param[in] highLimit The new high limit position that should be set in the hardware. Units=steps.*/ +asynStatus motorSimAxis::setHighLimit(double highLimit) +{ + hiHardLimit_ = highLimit; + return asynSuccess; +} + + +/** Set the low limit position of the motor. + * \param[in] lowLimit The new low limit position that should be set in the hardware. Units=steps.*/ +asynStatus motorSimAxis::setLowLimit(double lowLimit) +{ + lowHardLimit_ = lowLimit; + return asynSuccess; +} + + /** Process one iteration of an axis This routine takes a single axis and propogates its motion forward a given amount @@ -407,14 +425,16 @@ void motorSimAxis::process(double delta ) endpoint_.axis[0].p = home_; endpoint_.axis[0].v = 0.0; } + + route_pars_t pars; + routeGetParams(route_, &pars); + 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; + stop(pars.axis[0].Amax); } } else if (nextpoint_.axis[0].p < lowHardLimit_ && nextpoint_.axis[0].v < 0) @@ -422,9 +442,7 @@ void motorSimAxis::process(double delta ) 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; + stop(pars.axis[0].Amax); } } diff --git a/motorApp/MotorSimSrc/motorSimDriver.h b/motorApp/MotorSimSrc/motorSimDriver.h index 9457c4acf..18b2d42bb 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.h +++ b/motorApp/MotorSimSrc/motorSimDriver.h @@ -31,6 +31,9 @@ class epicsShareClass motorSimAxis : public asynMotorAxis asynStatus stop(double acceleration); asynStatus poll(bool *moving); asynStatus setPosition(double position); + asynStatus setHighLimit(double highLimit); + asynStatus setLowLimit(double lowLimit); + /* These are the methods that are new to this class */ asynStatus config(int hiHardLimit, int lowHardLimit, int home, int start); From bda53b0de269ec9a8929e319ae3dfd32c479870b Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 20 Jan 2017 14:40:04 +0000 Subject: [PATCH 045/232] Update for new ISIS_CONFIG naming --- configure/RELEASE | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/configure/RELEASE b/configure/RELEASE index 5c95195b8..524672e89 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -7,4 +7,5 @@ include $(TOP)/../../../configure/MASTER_RELEASE # optional extra local definitions here -include $(TOP)/configure/RELEASE.private -include $(TOP)/../../../ISIS_CONFIG.$(EPICS_HOST_ARCH) +include $(TOP)/../../../ISIS_CONFIG +-include $(TOP)/../../../ISIS_CONFIG.$(EPICS_HOST_ARCH) From 696734e50448b45c4a03c446840c4bdeee641435 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 2 Feb 2017 11:51:30 +0000 Subject: [PATCH 046/232] Choose homing behaviour based on (hard-coded currently) mode --- motorApp/MclennanSrc/devPM304.cc | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 4a32bf9c6..13263e027 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -25,6 +25,7 @@ #include +#include #include "motorRecord.h" #include "motor.h" #include "motordevCom.h" @@ -221,6 +222,8 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo break; } + int home_direction = 1; + switch (command) { case MOVE_ABS: @@ -229,17 +232,23 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo case MOVE_REL: sprintf(buff, "%dMR%ld;", axis, ival); break; - case HOME_FOR: - if (cntrl->model == MODEL_PM304){ - sprintf(buff, "%dIX;", axis); - } - /* Homing for PM600 done via SNL. See homing.st */ - break; case HOME_REV: + home_direction = -1; + case HOME_FOR: if (cntrl->model == MODEL_PM304){ - sprintf(buff, "%dIX-1;", axis); + sprintf(buff, "%dIX%d;", axis, home_direction); } - /* Homing for PM600 done via SNL. See homing.st */ + else { + int motor_default=0, constant_velocity=1, n_modes = 2; + //int band = pow(double(n_modes), axis-1); + //int home_mode = int((1 % band)/band); + int home_mode = 1; + if ( home_mode==motor_default ) { + sprintf(buff, "%dHD%d;", axis, home_direction); + } else { + // Let SNL take care of it + } + } break; case LOAD_POS: if (cntrl->use_encoder[axis-1]){ From 7bda381d562a91f7e42730d392b7a193660059ed Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 2 Feb 2017 11:52:06 +0000 Subject: [PATCH 047/232] Make mode integer. Move based on mode --- motorApp/MclennanSrc/homing.st | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index ccd2f6155..af7f4430a 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -1,4 +1,4 @@ -program homing("MOTPV=xxx,MODE=Constant_velocity") +program homing("MOTPV=xxx,MODE=0") #include "seqPVmacros.h" @@ -23,8 +23,8 @@ ss motor { when () { - /* Currently we only have one mode so it isn't used. */ - mode = "{MODE}"; + /* 0: Use motor home, 1: Constant velocity move */ + mode = 1; } state ready } @@ -42,16 +42,20 @@ ss motor state forward_home_requested { when (home_forward_pv==0) { - jog_forward_value = 1; - pvPut(jog_forward_value); + if ( mode==1 ) { + jog_forward_value = 1; + pvPut(jog_forward_value); + } } state moving } state reverse_home_requested { when (home_reverse_pv==0) { - jog_reverse_value = 1; - pvPut(jog_reverse_value); + if ( mode==1 ) { + jog_reverse_value = 1; + pvPut(jog_reverse_value); + } } state moving } From c8ea6c5a6f21b266ee81e3595172aea6ebd70737 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 2 Feb 2017 13:08:40 +0000 Subject: [PATCH 048/232] Reset position to 0 on default home --- motorApp/MclennanSrc/devPM304.cc | 2 +- motorApp/MclennanSrc/homing.st | 27 +++++++++++++++++++++++---- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 13263e027..a14a67771 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -242,7 +242,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo int motor_default=0, constant_velocity=1, n_modes = 2; //int band = pow(double(n_modes), axis-1); //int home_mode = int((1 % band)/band); - int home_mode = 1; + int home_mode = 0; if ( home_mode==motor_default ) { sprintf(buff, "%dHD%d;", axis, home_direction); } else { diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index af7f4430a..7b72cd542 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -1,13 +1,20 @@ -program homing("MOTPV=xxx,MODE=0") +program homing("MOTPV=xxx,MODE") #include "seqPVmacros.h" char* SNLtaskName; -char* mode; int jog_forward_value, jog_reverse_value; +char* macros; assign jog_forward_value to "{MOTPV}.JOGF"; assign jog_reverse_value to "{MOTPV}.JOGR"; + +int set, position, position_d; +assign set to "{MOTPV}.SET"; +assign position to "{MOTPV}.VAL"; +assign position_d to "{MOTPV}.DVAL"; + int debug_flag; +int mode; /* Turn on run-time debug messages */ option +d; @@ -23,8 +30,8 @@ ss motor { when () { - /* 0: Use motor home, 1: Constant velocity move */ - mode = 1; + /* "0": Use motor home, "1": Constant velocity move */ + mode = atoi(macValueGet("MODE")); } state ready } @@ -63,6 +70,18 @@ ss motor { when (movable==1) { + if ( mode==0 ) { + set = 1; + position = 0; + position_d = 0; + + pvPut(set); + pvPut(position); + pvPut(position_d); + + set = 0; + pvPut(set); + } } state ready } } From 050b31b585f01b22c7af0177d736212b7db11e32 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 2 Feb 2017 13:51:21 +0000 Subject: [PATCH 049/232] Add reverse home and 0 mode --- motorApp/MclennanSrc/devPM304.cc | 11 +++++++---- motorApp/MclennanSrc/homing.st | 5 +++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index a14a67771..834742a8d 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -239,14 +239,17 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dIX%d;", axis, home_direction); } else { - int motor_default=0, constant_velocity=1, n_modes = 2; + int motor_default=0, constant_velocity=1, reverse_and_zero=2, n_modes = 3; //int band = pow(double(n_modes), axis-1); //int home_mode = int((1 % band)/band); - int home_mode = 0; - if ( home_mode==motor_default ) { + int home_mode = 2; + if ( home_mode==motor_default || home_mode==reverse_and_zero ) { + if ( home_mode==reverse_and_zero ) { + home_direction = -1; + } sprintf(buff, "%dHD%d;", axis, home_direction); } else { - // Let SNL take care of it + // Let SNL take care of everything. See homing.st } } break; diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 7b72cd542..80d859210 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -30,8 +30,9 @@ ss motor { when () { - /* "0": Use motor home, "1": Constant velocity move */ + /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0 */ mode = atoi(macValueGet("MODE")); + printf("Homing sequence move mode set to %i\n", mode); } state ready } @@ -70,7 +71,7 @@ ss motor { when (movable==1) { - if ( mode==0 ) { + if ( mode==2 ) { set = 1; position = 0; position_d = 0; From 7e44e90ad15f0a84a19fc9c2db68de25f5dd4322 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 2 Feb 2017 13:51:31 +0000 Subject: [PATCH 050/232] Pass in mode from IOC --- motorApp/MclennanSrc/MclennanRegister.cc | 9 +++++---- motorApp/MclennanSrc/devPM304.cc | 9 +++++---- motorApp/MclennanSrc/drvPM304.cc | 4 +++- motorApp/MclennanSrc/drvPM304.h | 3 ++- 4 files changed, 15 insertions(+), 10 deletions(-) diff --git a/motorApp/MclennanSrc/MclennanRegister.cc b/motorApp/MclennanSrc/MclennanRegister.cc index 1563bdf9a..0dda3ff9c 100644 --- a/motorApp/MclennanSrc/MclennanRegister.cc +++ b/motorApp/MclennanSrc/MclennanRegister.cc @@ -33,13 +33,14 @@ static const iocshArg setupArg1 = {"Polling rate", iocshArgInt}; static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; static const iocshArg configArg1 = {"asyn port name", iocshArgString}; static const iocshArg configArg2 = {"Number of axes", iocshArgInt}; +static const iocshArg configArg3 = {"Combined home modes for all axes", iocshArgInt}; static const iocshArg * const PM304SetupArgs[2] = {&setupArg0, &setupArg1}; -static const iocshArg * const PM304ConfigArgs[3] = {&configArg0, &configArg1, - &configArg2}; +static const iocshArg * const PM304ConfigArgs[4] = {&configArg0, &configArg1, + &configArg2, &configArg3}; static const iocshFuncDef setupPM304 = {"PM304Setup", 2, PM304SetupArgs}; -static const iocshFuncDef configPM304 = {"PM304Config", 3, PM304ConfigArgs}; +static const iocshFuncDef configPM304 = {"PM304Config", 4, PM304ConfigArgs}; static void setupPM304CallFunc(const iocshArgBuf *args) { @@ -47,7 +48,7 @@ static void setupPM304CallFunc(const iocshArgBuf *args) } static void configPM304CallFunc(const iocshArgBuf *args) { - PM304Config(args[0].ival, args[1].sval, args[2].ival); + PM304Config(args[0].ival, args[1].sval, args[2].ival, args[3].ival); } static void MclennanRegister(void) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 834742a8d..994a16ad9 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -239,10 +239,11 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dIX%d;", axis, home_direction); } else { - int motor_default=0, constant_velocity=1, reverse_and_zero=2, n_modes = 3; - //int band = pow(double(n_modes), axis-1); - //int home_mode = int((1 % band)/band); - int home_mode = 2; + int motor_default=0, constant_velocity=1, reverse_and_zero=2; + // Based on 32-bit integer, splitting across 8 axis. 4 bits -> 8 modes + int n_modes = 8; + int band = pow(double(n_modes), axis-1); + int home_mode = int(cntrl->home_modes/float(band)) % band; if ( home_mode==motor_default || home_mode==reverse_and_zero ) { if ( home_mode==reverse_and_zero ) { home_direction = -1; diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 273992cd4..3dd7f0c9d 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -566,7 +566,8 @@ PM304Setup(int num_cards, /* maximum number of controllers in system */ RTN_STATUS PM304Config(int card, /* card being configured */ const char *port, /* asyn port name */ - int n_axes) /* Number of axes */ + int n_axes, /* Number of axes */ + int home_modes) /* Combined home modes of all axes */ { struct PM304controller *cntrl; @@ -578,6 +579,7 @@ PM304Config(int card, /* card being configured */ motor_state[card]->DevicePrivate = malloc(sizeof(struct PM304controller)); cntrl = (struct PM304controller *) motor_state[card]->DevicePrivate; cntrl->n_axes = n_axes; + cntrl->home_modes = home_modes; cntrl->model = MODEL_PM304; /* Assume PM304 initially */ strcpy(cntrl->port, port); return(OK); diff --git a/motorApp/MclennanSrc/drvPM304.h b/motorApp/MclennanSrc/drvPM304.h index 2d939190f..5a35fc7be 100644 --- a/motorApp/MclennanSrc/drvPM304.h +++ b/motorApp/MclennanSrc/drvPM304.h @@ -36,10 +36,11 @@ struct PM304controller int n_axes; /* Number of axes on this controller */ int model; /* Model = MODEL_PM304 or MODEL_PM600 */ int use_encoder[PM304_MAX_CHANNELS]; /* Does axis have an encoder? */ + int home_modes; /* The combined home modes for all axes */ char port[80]; /* asyn port name */ }; RTN_STATUS PM304Setup(int, int); -RTN_STATUS PM304Config(int, const char *, int); +RTN_STATUS PM304Config(int, const char *, int, int); #endif /* INCdrvPM304h */ From 1e9a8e130e1599886f3523d06c88e6abe5c8cd83 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 2 Feb 2017 14:38:57 +0000 Subject: [PATCH 051/232] Add some extra debug output --- motorApp/MclennanSrc/devPM304.cc | 3 ++- motorApp/MclennanSrc/homing.st | 7 ++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 994a16ad9..2eef6e269 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -243,7 +243,8 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo // Based on 32-bit integer, splitting across 8 axis. 4 bits -> 8 modes int n_modes = 8; int band = pow(double(n_modes), axis-1); - int home_mode = int(cntrl->home_modes/float(band)) % band; + int home_mode = int(cntrl->home_modes/float(band)) % n_modes; + printf("Homing axis %i using mode %i\n", axis, home_mode); if ( home_mode==motor_default || home_mode==reverse_and_zero ) { if ( home_mode==reverse_and_zero ) { home_direction = -1; diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 80d859210..3fbbbf69a 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -1,10 +1,9 @@ -program homing("MOTPV=xxx,MODE") +program homing("MOTPV=xxx,MODE,AXIS") #include "seqPVmacros.h" char* SNLtaskName; int jog_forward_value, jog_reverse_value; -char* macros; assign jog_forward_value to "{MOTPV}.JOGF"; assign jog_reverse_value to "{MOTPV}.JOGR"; @@ -15,6 +14,7 @@ assign position_d to "{MOTPV}.DVAL"; int debug_flag; int mode; +int axis; /* Turn on run-time debug messages */ option +d; @@ -32,7 +32,8 @@ ss motor { /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0 */ mode = atoi(macValueGet("MODE")); - printf("Homing sequence move mode set to %i\n", mode); + axis = atoi(macValueGet("AXIS")); + printf("Sequencer: Homing mode for axis %i set to %i\n", axis, mode); } state ready } From b63e7b5831cb71ade99a757ed6bca755406d8543 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Thu, 2 Feb 2017 16:38:49 +0000 Subject: [PATCH 052/232] Calculate homing modes at config rather than at each homing call --- motorApp/MclennanSrc/devPM304.cc | 7 +------ motorApp/MclennanSrc/drvPM304.cc | 8 +++++++- motorApp/MclennanSrc/drvPM304.h | 2 +- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 2eef6e269..6bf92ea1d 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -25,7 +25,6 @@ #include -#include #include "motorRecord.h" #include "motor.h" #include "motordevCom.h" @@ -240,11 +239,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo } else { int motor_default=0, constant_velocity=1, reverse_and_zero=2; - // Based on 32-bit integer, splitting across 8 axis. 4 bits -> 8 modes - int n_modes = 8; - int band = pow(double(n_modes), axis-1); - int home_mode = int(cntrl->home_modes/float(band)) % n_modes; - printf("Homing axis %i using mode %i\n", axis, home_mode); + int home_mode = cntrl->home_mode[axis-1]; if ( home_mode==motor_default || home_mode==reverse_and_zero ) { if ( home_mode==reverse_and_zero ) { home_direction = -1; diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 3dd7f0c9d..6e0a49058 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -32,6 +32,7 @@ #include +#include #include #include #include @@ -579,7 +580,12 @@ PM304Config(int card, /* card being configured */ motor_state[card]->DevicePrivate = malloc(sizeof(struct PM304controller)); cntrl = (struct PM304controller *) motor_state[card]->DevicePrivate; cntrl->n_axes = n_axes; - cntrl->home_modes = home_modes; + for (int i=0; i 8 modes + int n_modes = 8; + cntrl->home_mode[i] = int(home_modes/float(pow(double(n_modes), i)))%n_modes; + printf("Homing axis %i using mode %i\n", i+1, cntrl->home_mode[i]); + } cntrl->model = MODEL_PM304; /* Assume PM304 initially */ strcpy(cntrl->port, port); return(OK); diff --git a/motorApp/MclennanSrc/drvPM304.h b/motorApp/MclennanSrc/drvPM304.h index 5a35fc7be..ace4bdf06 100644 --- a/motorApp/MclennanSrc/drvPM304.h +++ b/motorApp/MclennanSrc/drvPM304.h @@ -36,7 +36,7 @@ struct PM304controller int n_axes; /* Number of axes on this controller */ int model; /* Model = MODEL_PM304 or MODEL_PM600 */ int use_encoder[PM304_MAX_CHANNELS]; /* Does axis have an encoder? */ - int home_modes; /* The combined home modes for all axes */ + int home_mode[PM304_MAX_CHANNELS]; /* The combined home modes for all axes */ char port[80]; /* asyn port name */ }; From c9574b17e734eca996249f9830100d550bdac674 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 14 Feb 2017 14:40:10 +0000 Subject: [PATCH 053/232] Add config xml with IOC description --- iocBoot/iocmotorSim/config.xml | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 iocBoot/iocmotorSim/config.xml diff --git a/iocBoot/iocmotorSim/config.xml b/iocBoot/iocmotorSim/config.xml new file mode 100644 index 000000000..32b076052 --- /dev/null +++ b/iocBoot/iocmotorSim/config.xml @@ -0,0 +1,6 @@ + + + +Simulated motor + + From bd9a95460e7de46d1297800c3d9ce56dc706870b Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Mon, 6 Mar 2017 14:17:20 +0000 Subject: [PATCH 054/232] Refactor homing code into separate method --- motorApp/MclennanSrc/devPM304.cc | 39 ++++++++++++++++---------------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 6bf92ea1d..78d80afbf 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -160,6 +160,22 @@ STATIC RTN_STATUS PM304_end_trans(struct motorRecord *mr) } +/* request homing move */ +STATIC void request_home(char* buff, int model, int axis, int home_direction, int home_mode) { + if (model == MODEL_PM304){ + sprintf(buff, "%dIX%d;", axis, home_direction); + } else { + int motor_default=0, constant_velocity=1, reverse_and_zero=2; + if ( home_mode==motor_default || home_mode==reverse_and_zero ) { + if ( home_mode==reverse_and_zero ) { + home_direction = -1; + } + sprintf(buff, "%dHD%d;", axis, home_direction); + } else { + // Let SNL take care of everything. See homing.st + } + } +} /* add a part to the transaction */ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) @@ -220,9 +236,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo default: break; } - - int home_direction = 1; - + switch (command) { case MOVE_ABS: @@ -232,23 +246,10 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dMR%ld;", axis, ival); break; case HOME_REV: - home_direction = -1; + request_home(buff, cntrl->model, axis, -1, cntrl->home_mode[axis-1]); + break; case HOME_FOR: - if (cntrl->model == MODEL_PM304){ - sprintf(buff, "%dIX%d;", axis, home_direction); - } - else { - int motor_default=0, constant_velocity=1, reverse_and_zero=2; - int home_mode = cntrl->home_mode[axis-1]; - if ( home_mode==motor_default || home_mode==reverse_and_zero ) { - if ( home_mode==reverse_and_zero ) { - home_direction = -1; - } - sprintf(buff, "%dHD%d;", axis, home_direction); - } else { - // Let SNL take care of everything. See homing.st - } - } + request_home(buff, cntrl->model, axis, 1, cntrl->home_mode[axis-1]); break; case LOAD_POS: if (cntrl->use_encoder[axis-1]){ From e9a3e6621136da21c58adfb64fe7ec84b1979391 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 28 Feb 2017 12:06:07 +0000 Subject: [PATCH 055/232] Clear previous homes/jogs --- motorApp/MclennanSrc/homing.st | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 3fbbbf69a..c7f45f197 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -52,6 +52,13 @@ ss motor { when (home_forward_pv==0) { if ( mode==1 ) { + /* Clear any outstanding jogs */ + jog_reverse_value = 0; + pvPut(jog_reverse_value); + jog_forward_value = 0; + pvPut(jog_forward_value); + + /* New jog */ jog_forward_value = 1; pvPut(jog_forward_value); } @@ -62,6 +69,13 @@ ss motor { when (home_reverse_pv==0) { if ( mode==1 ) { + /* Clear any outstanding jogs */ + jog_reverse_value = 0; + pvPut(jog_reverse_value); + jog_forward_value = 0; + pvPut(jog_forward_value); + + /* New jog */ jog_reverse_value = 1; pvPut(jog_reverse_value); } From 15e3fd92090f6dc245d477acea918e90cee2cc40 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 28 Feb 2017 14:29:04 +0000 Subject: [PATCH 056/232] Clear jog at end of home --- motorApp/MclennanSrc/homing.st | 37 +++++++++++++++++----------------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index c7f45f197..f07162c4f 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -38,7 +38,7 @@ ss motor } state ready - { + { when (home_reverse_pv==1) { } state reverse_home_requested @@ -52,33 +52,25 @@ ss motor { when (home_forward_pv==0) { if ( mode==1 ) { - /* Clear any outstanding jogs */ - jog_reverse_value = 0; - pvPut(jog_reverse_value); - jog_forward_value = 0; - pvPut(jog_forward_value); - - /* New jog */ jog_forward_value = 1; pvPut(jog_forward_value); } - } state moving + } state processing_jog_request } state reverse_home_requested { when (home_reverse_pv==0) { - if ( mode==1 ) { - /* Clear any outstanding jogs */ - jog_reverse_value = 0; - pvPut(jog_reverse_value); - jog_forward_value = 0; - pvPut(jog_forward_value); - - /* New jog */ + if ( mode==1 ) { jog_reverse_value = 1; pvPut(jog_reverse_value); } + } state processing_jog_request + } + + state processing_jog_request + { + when (movable==0) { } state moving } @@ -98,6 +90,15 @@ ss motor set = 0; pvPut(set); } - } state ready + } state done } + + state done { + when () { + jog_reverse_value = 0; + pvPut(jog_reverse_value); + jog_forward_value = 0; + pvPut(jog_forward_value); + } state ready + } } From bc293a955429eb3cd0ccfc9a81f50c5104a691e1 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 21 Mar 2017 09:37:14 +0000 Subject: [PATCH 057/232] Add preliminary LinMot code --- motorApp/LinMotSrc/Makefile | 19 + motorApp/LinMotSrc/devLinMot.cc | 111 ++++++ motorApp/LinMotSrc/devLinMotMotor.dbd | 5 + motorApp/LinMotSrc/drvLinMot.cc | 493 ++++++++++++++++++++++++++ motorApp/LinMotSrc/drvLinMot.h | 27 ++ 5 files changed, 655 insertions(+) create mode 100644 motorApp/LinMotSrc/Makefile create mode 100644 motorApp/LinMotSrc/devLinMot.cc create mode 100644 motorApp/LinMotSrc/devLinMotMotor.dbd create mode 100644 motorApp/LinMotSrc/drvLinMot.cc create mode 100644 motorApp/LinMotSrc/drvLinMot.h diff --git a/motorApp/LinMotSrc/Makefile b/motorApp/LinMotSrc/Makefile new file mode 100644 index 000000000..3a834c9e3 --- /dev/null +++ b/motorApp/LinMotSrc/Makefile @@ -0,0 +1,19 @@ +# Makefile +TOP = ../.. +include $(TOP)/configure/CONFIG + +# The following are used for debugging messages. +#!USR_CXXFLAGS += -DDEBUG + +DBD += devLinMotMotor.dbd + +LIBRARY_IOC = LinMotMotor + +# Khozu SC-800 motor controller. +SRCS += devLinMot.cc drvLinMot.cc + +LinMotMotor_LIBS += motor asyn +LinMotMotor_LIBS += $(EPICS_BASE_IOC_LIBS) + +include $(TOP)/configure/RULES + diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc new file mode 100644 index 000000000..98e443251 --- /dev/null +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -0,0 +1,111 @@ +#include +#include +#include "motorRecord.h" +#include "motor.h" +#include "motordevCom.h" +#include "drvLinMot.h" +#include "epicsExport.h" + +extern struct driver_table LinMot_access; + +/* ----------------Create the dsets for devLinMot----------------- */ +static struct driver_table *drvtabptr; +static long LinMot_init(void *); +static long LinMot_init_record(void *); +static long LinMot_start_trans(struct motorRecord *); +static RTN_STATUS LinMot_build_trans(motor_cmnd, double *, struct motorRecord *); +static RTN_STATUS LinMot_end_trans(struct motorRecord *); + +struct motor_dset devLinMot = +{ + {8, NULL, (DEVSUPFUN) LinMot_init, (DEVSUPFUN) LinMot_init_record, NULL}, + motor_update_values, + LinMot_start_trans, + LinMot_build_trans, + LinMot_end_trans +}; + +extern "C" {epicsExportAddress(dset,devLinMot);} + +/* --------------------------- program data --------------------- */ + +/* This table is used to define the command types */ +/* WARNING! this must match "motor_cmnd" in motor.h */ + +static msg_types LinMot_table[] = { + MOTION, /* MOVE_ABS */ + MOTION, /* MOVE_REL */ + MOTION, /* HOME_FOR */ + MOTION, /* HOME_REV */ + IMMEDIATE, /* LOAD_POS */ + IMMEDIATE, /* SET_VEL_BASE */ + IMMEDIATE, /* SET_VELOCITY */ + IMMEDIATE, /* SET_ACCEL */ + IMMEDIATE, /* GO */ + IMMEDIATE, /* SET_ENC_RATIO */ + INFO, /* GET_INFO */ + MOVE_TERM, /* STOP_AXIS */ + VELOCITY, /* JOG */ + IMMEDIATE, /* SET_PGAIN */ + IMMEDIATE, /* SET_IGAIN */ + IMMEDIATE, /* SET_DGAIN */ + IMMEDIATE, /* ENABLE_TORQUE */ + IMMEDIATE, /* DISABL_TORQUE */ + IMMEDIATE, /* PRIMITIVE */ + IMMEDIATE, /* SET_HIGH_LIMIT */ + IMMEDIATE, /* SET_LOW_LIMIT */ + VELOCITY, /* JOG_VELOCITY */ + IMMEDIATE /* SET_RESOLUTION */ +}; + +static inline void Debug(int level, const char *format, ...) { + #ifdef DEBUG + if (level < devLinMotDebug) { + va_list pVar; + va_start(pVar, format); + vprintf(format, pVar); + va_end(pVar); + } + #endif +} + + +static struct board_stat **LinMot_cards; + +/* --------------------------- program data --------------------- */ + + +/* initialize device support for LinMot stepper motor */ +static long LinMot_init(void *arg) +{ + return(OK); +} + + +/* initialize a record instance */ +static long LinMot_init_record(void *arg) +{ + return(OK); +} + + +/* start building a transaction */ +static long LinMot_start_trans(struct motorRecord *mr) +{ + return(OK); +} + + +/* end building a transaction */ +static RTN_STATUS LinMot_end_trans(struct motorRecord *mr) +{ + return(OK); +} + + +/* add a part to the transaction */ +static RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) +{ + return(OK); +} + diff --git a/motorApp/LinMotSrc/devLinMotMotor.dbd b/motorApp/LinMotSrc/devLinMotMotor.dbd new file mode 100644 index 000000000..6de3f1f30 --- /dev/null +++ b/motorApp/LinMotSrc/devLinMotMotor.dbd @@ -0,0 +1,5 @@ +# LinMot motor controller support. +device(motor,VME_IO, devLinMot, "LinMot motor") +driver(drvLinMot) +registrar(LinMotRegister) +variable(drvLinMotDebug) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc new file mode 100644 index 000000000..867a0a985 --- /dev/null +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -0,0 +1,493 @@ +#include +#include /* isascii functions */ +#include +#include +#include +#include +#include +#include +#include "motor.h" +#include "motorRecord.h" +#include "drvLinMot.h" +#include "asynOctetSyncIO.h" +#include "epicsExport.h" + +#define BUFF_SIZE 120 /* Maximum length of string to/from LinMot */ + +#define TIMEOUT 3.0 /* Command timeout in sec. */ + +#define LINMOT_MAX_CARDS 4 + +/* Delay after START_MOTION before a status update is possible */ +#define MOTION_DELAY 0.1 + +/*----------------debugging-----------------*/ +volatile int drvLinMotDebug = 0; +extern "C" {epicsExportAddress(int, drvLinMotDebug);} + +static inline void Debug(int level, const char *format, ...) { + if (level < drvLinMotDebug) { + va_list pVar; + va_start(pVar, format); + vprintf(format, pVar); + va_end(pVar); + } +} + +/* Local data required for every driver; see "motordrvComCode.h" */ +#include "motordrvComCode.h" + + +/* This is a temporary fix to introduce a delayed reading of the motor + * position after a move completes + */ +volatile double drvLinMotReadbackDelay = 0.; + +int LinMot_num_cards = 0; + + +/*----------------functions-----------------*/ +static int recv_mess(int card, char *com, int flag); +static RTN_STATUS send_mess(int card, char const *, char *name); +static int set_status(int card, int signal); +static long report(int level); +static long init(); +static int motor_init(); +static void query_done(int, int, struct mess_node *); + +/*----------------functions-----------------*/ + +struct driver_table LinMot_access = +{ + motor_init, + motor_send, + motor_free, + motor_card_info, + motor_axis_info, + &mess_queue, + &queue_lock, + &free_list, + &freelist_lock, + &motor_sem, + &motor_state, + &total_cards, + &any_motor_in_motion, + send_mess, + recv_mess, + set_status, + query_done, + NULL, + &initialized, + NULL +}; + +struct drvLinMot_drvet +{ + long number; + long (*report) (int); + long (*init) (void); +} drvLinMot = {2, report, init}; + +extern "C" {epicsExportAddress(drvet, drvLinMot);} + +static struct thread_args targs = {SCAN_RATE, &LinMot_access, MOTION_DELAY}; + +/********************************************************* + * Print out driver status report + *********************************************************/ +static long report(int level) +{ + int card; + + if (LinMot_num_cards <=0) + printf("[WARNING] No LinMot controllers found\n"); + else + { + for (card = 0; card < LinMot_num_cards; card++) + if (motor_state[card]) + printf("LinMot controller %d, id: %s \n", + card, motor_state[card]->ident); + } + return (0); +} + + +static long init() +{ + if (LinMot_num_cards <= 0) + { + Debug(1, "LinMotSetup() is missing from startup script.\n"); + return (ERROR); + } + return (OK); +} + + +static void query_done(int card, int axis, struct mess_node *nodeptr) +{ +} + +static int send_recv_mess(int card, const char *out, char *response) +{ + char *p, *tok_save; + struct LinMotController *cntrl; + asynStatus status; + size_t nwrite=0, nread=0; + int eomReason; + char temp[BUFF_SIZE]; + + response[0] = '\0'; + + /* Check that card exists */ + if (!motor_state[card]) + { + epicsPrintf("send_recv_mess - invalid card #%d\n", card); + return (-1); + } + + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + + /* Device support can send us multiple commands separated with ';' + * characters. The LinMot cannot handle more than 1 command on a line + * so send them separately */ + strcpy(temp, out); + for (p = epicsStrtok_r(temp, ";", &tok_save); + ((p != NULL) && (strlen(p) != 0)); + p = epicsStrtok_r(NULL, ";", &tok_save)) { + Debug(2, "send_recv_mess: sending message to card %d, message=%s\n", card, p); + status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), + response, BUFF_SIZE, TIMEOUT, + &nwrite, &nread, &eomReason); + } + + if (nread == 0) response[0] = '\0'; + if (nread > 0) { + Debug(2, "send_recv_mess: card %d, response = \"%s\"\n", card, response); + } + if (nread == 0) { + Debug(1, "send_recv_mess: card %d ERROR: no response\n", card); + } + return(strlen(response)); +} + + +/************************************************************** + * Parse status and position strings for a card and signal + * set_status() + ************************************************************/ + +static int set_status(int card, int signal) +{ + return (0); +} + + +/*****************************************************/ +/* send a message to the LinMot board */ +/* send_mess() */ +/*****************************************************/ +static RTN_STATUS send_mess(int card, char const *com, char *name) +{ + char *p, *tok_save; + char response[BUFF_SIZE]; + char temp[BUFF_SIZE]; + struct LinMotController *cntrl; + size_t nwrite, nread; + int eomReason; + + /* Check that card exists */ + if (!motor_state[card]) + { + epicsPrintf("send_mess - invalid card #%d\n", card); + return(ERROR); + } + + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + + /* Device support can send us multiple commands separated with ';' + * characters. The LinMot cannot handle more than 1 command on a line + * so send them separately */ + strcpy(temp, com); + for (p = epicsStrtok_r(temp, ";", &tok_save); + ((p != NULL) && (strlen(p) != 0)); + p = epicsStrtok_r(NULL, ";", &tok_save)) { + Debug(2, "send_mess: sending message to card %d, message=%s\n", card, p); + pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, + BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); + Debug(2, "send_mess: card %d, response=%s\n", card, response); + } + return(OK); +} + + +/* + * FUNCTION... recv_mess(int card, char *com, int flag) + * + * INPUT ARGUMENTS... + * card - controller card # (0,1,...). + * *com - caller's response buffer. + * flag | FLUSH = this flag is ignored - the receive buffer is flushed + * on every write (see write_mess()) + * LOGIC... + * IF controller card does not exist. + * ERROR RETURN. + * ENDIF + * NORMAL RETURN. + */ + +static int recv_mess(int card, char *com, int flag) +{ + double timeout; + char *pos; + char temp[BUFF_SIZE]; + int flush; + asynStatus status; + size_t nread=0; + int eomReason; + struct LinMotController *cntrl; + + com[0] = '\0'; + /* Check that card exists */ + if (!motor_state[card]) + { + epicsPrintf("resv_mess - invalid card #%d\n", card); + return(-1); + } + + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + + if (flag == FLUSH) { + flush = 1; + timeout = 0; + } else { + flush = 0; + timeout = TIMEOUT; + } + if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser); + status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, + timeout, &nread, &eomReason); + + /* The response from the PM304 is terminated with CR/LF. Remove these */ + if (nread == 0) com[0] = '\0'; + if (nread > 0) { + Debug(2, "recv_mess: card %d, flag=%d, message = \"%s\"\n", card, flag, com); + } + if (nread == 0) { + if (flag != FLUSH) { + Debug(1, "recv_mess: card %d read ERROR: no response\n", card); + } else { + Debug(3, "recv_mess: card %d flush returned no characters\n", card); + } + } + return(strlen(com)); +} + + +/*****************************************************/ +/* Setup system configuration */ +/* LinMotSetup() */ +/*****************************************************/ +RTN_STATUS +LinMotSetup(int num_cards, /* maximum number of controllers in system. */ + int scan_rate) /* polling rate - 1/60 sec units. */ +{ + if (num_cards < 1 || num_cards > LINMOT_MAX_CARDS) + LinMot_num_cards = LINMOT_MAX_CARDS; + else + LinMot_num_cards = num_cards; + + /* Set motor polling task rate */ + if (scan_rate >= 1 && scan_rate <= 60) + targs.motor_scan_rate = scan_rate; + else + targs.motor_scan_rate = SCAN_RATE; + + /* + * Allocate space for motor_state structure pointers. Note this must be done + * before LinMotConfig is called, so it cannot be done in motor_init() + * This means that we must allocate space for a card without knowing + * if it really exists, which is not a serious problem since this is just + * an array of pointers. + */ + motor_state = (struct controller **) malloc(LinMot_num_cards * + sizeof(struct controller *)); + + for (int card = 0; card < LinMot_num_cards; card++) + motor_state[card] = (struct controller *) NULL; + + Debug(3, "LinMotSetup completed successfully\n"); + return(OK); +} + + +/*****************************************************/ +/* Configure a controller */ +/* LinMotConfig() */ +/*****************************************************/ +RTN_STATUS +LinMotConfig(int card, /* card being configured */ + const char *port, /* asyn port name */ + int n_axes /* Number of axes */ + ) +{ + struct LinMotController *cntrl; + if (card < 0 || card >= LinMot_num_cards) + return (ERROR); + + motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); + motor_state[card]->DevicePrivate = malloc(sizeof(struct LinMotController)); + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + cntrl->n_axes = n_axes; + strcpy(cntrl->port, port); + Debug(3, "LinMotConfig completed successfully\n"); + return(OK); +} + + + +/*****************************************************/ +/* initialize all software and hardware */ +/* This is called from the initialization routine in */ +/* device support. */ +/* motor_init() */ +/*****************************************************/ +static int motor_init() +{ + struct controller *brdptr; + struct LinMotController *cntrl; + int card_index, motor_index; + char buff[BUFF_SIZE]; + char command[20]; + int total_axis = 0; + int status; + bool success_rtn; + + initialized = true; /* Indicate that driver is initialized. */ + + /* Check for setup */ + if (LinMot_num_cards <= 0) + { + Debug(1, "LinMotSetup() is missing from startup script.\n"); + return (ERROR); + } + + for (card_index = 0; card_index < LinMot_num_cards; card_index++) + { + if (!motor_state[card_index]) + continue; + + brdptr = motor_state[card_index]; + total_cards = card_index + 1; + cntrl = (struct LinMotController *) brdptr->DevicePrivate; + + /* Initialize communications channel */ + success_rtn = false; + + status = pasynOctetSyncIO->connect(cntrl->port, 0, &cntrl->pasynUser, NULL); + success_rtn = (status == asynSuccess); + Debug(1, "motor_init, return from pasynOctetSyncIO->connect for port %s = %d, pasynUser=%p\n", cntrl->port, success_rtn, cntrl->pasynUser); + + if (success_rtn == true) + { + int retry = 0; + + /* Send a message to the board, see if it exists */ + /* flush any junk at input port - should not be any data available */ + do { + recv_mess(card_index, buff, FLUSH); + } while (strlen(buff) != 0); + + do + { + send_recv_mess(card_index, "!GPA", buff); + retry++; + /* Return value is length of response string */ + } while(strlen(buff) == 0 && retry < 3); + } + + if (success_rtn == true && strlen(buff) > 0) + { + brdptr->localaddr = (char *) NULL; + brdptr->motor_in_motion = 0; + /* Leave bdptr->cmnd_response false because we read each response */ + /* in send_mess and send_recv_mess. */ + brdptr->cmnd_response = false; + + /* Don't turn on motor power, too dangerous */ + /* send_mess(i, "1RSES;", buff); */ + send_mess(card_index, "!SS1A;", 0); /* Stop motor */ + send_recv_mess(card_index, "!PVA;", buff); /* Read controller ID string */ + strncpy(brdptr->ident, buff, MAX_IDENT_LEN); + + total_axis = cntrl->n_axes; + brdptr->total_axis = total_axis; + for (motor_index = 0; motor_index < total_axis; motor_index++) + { + struct mess_info *motor_info = &brdptr->motor_info[motor_index]; + + motor_info->motor_motion = NULL; + motor_info->status.All = 0; + motor_info->no_motion_count = 0; + motor_info->encoder_position = 0; + motor_info->position = 0; + set_status(card_index, motor_index); /* Read status of each motor */ + } + + } + else + motor_state[card_index] = (struct controller *) NULL; + } + + any_motor_in_motion = 0; + + Debug(3, "motor_init: spawning motor task\n"); + + mess_queue.head = (struct mess_node *) NULL; + mess_queue.tail = (struct mess_node *) NULL; + + free_list.head = (struct mess_node *) NULL; + free_list.tail = (struct mess_node *) NULL; + + epicsThreadCreate((char *) "tLinMot", epicsThreadPriorityMedium, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) motor_task, (void *) &targs); + + return(OK); +} + +extern "C" +{ + +// Setup arguments + static const iocshArg setupArg0 = {"Maximum # of cards", iocshArgInt}; + static const iocshArg setupArg1 = {"Polling rate (HZ)", iocshArgInt}; +// Config arguments + static const iocshArg configArg0 = {"Card# being configured", iocshArgInt}; + static const iocshArg configArg1 = {"asyn port name", iocshArgString}; + static const iocshArg configArg2 = {"asyn address (GPIB)", iocshArgInt}; + + static const iocshArg *const SetupArgs[2] = {&setupArg0, &setupArg1}; + static const iocshArg *const ConfigArgs[3] = {&configArg0, &configArg1, &configArg2}; + + static const iocshFuncDef setupLinMot = {"LinMotSetup", 2, SetupArgs}; + static const iocshFuncDef configLinMot = {"LinMotConfig", 3, ConfigArgs}; + + static void setupLinMotCallFunc(const iocshArgBuf *args) + { + LinMotSetup(args[0].ival, args[1].ival); + } + static void configLinMotCallFunc (const iocshArgBuf *args) + { + LinMotConfig(args[0].ival, args[1].sval, args[2].ival); + } + + static void LinMotRegister(void) + { + iocshRegister(&setupLinMot, setupLinMotCallFunc); + iocshRegister(&configLinMot, configLinMotCallFunc); + } + + epicsExportRegistrar(LinMotRegister); + +} // extern "C" + diff --git a/motorApp/LinMotSrc/drvLinMot.h b/motorApp/LinMotSrc/drvLinMot.h new file mode 100644 index 000000000..4faf62fcf --- /dev/null +++ b/motorApp/LinMotSrc/drvLinMot.h @@ -0,0 +1,27 @@ +#ifndef INCdrvLinMoth +#define INCdrvLinMoth 1 + +#include "motor.h" +#include "motordrvCom.h" +#include "asynDriver.h" +#include "asynOctetSyncIO.h" + +#define LinMot_MAX_MOTORS 4 +#define LinMot_MSG_SIZE 80 +#define LinMot_STATUS_RETRY 10 + +/* End-of-string defines */ +#define LinMot_OUT_EOS "\r\n" /* Command */ +#define LinMot_IN_EOS "\r\n" /* Reply */ + +/* Motion Master specific data is stored in this structure. */ +struct LinMotController +{ + asynUser *pasynUser; /* For RS-232 */ + char port[80]; /* asyn port name */ + int n_axes; /* Number of axes */ + int max_speed[LinMot_MAX_MOTORS]; /* steps/sec. */ +}; + +#endif /* INCdrvLinMoth */ + From 21dcb6d8ffaa7aee73e077a7355dc624443098a9 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 21 Mar 2017 10:51:05 +0000 Subject: [PATCH 058/232] Blank slate. Won't run, will compile --- motorApp/LinMotSrc/LinMotRegister.cc | 52 +++ motorApp/LinMotSrc/Makefile | 17 +- motorApp/LinMotSrc/devLinMot.cc | 81 +++-- motorApp/LinMotSrc/devLinMotMotor.dbd | 2 +- motorApp/LinMotSrc/drvLinMot.cc | 457 ++++---------------------- motorApp/LinMotSrc/drvLinMot.h | 45 ++- 6 files changed, 201 insertions(+), 453 deletions(-) create mode 100644 motorApp/LinMotSrc/LinMotRegister.cc diff --git a/motorApp/LinMotSrc/LinMotRegister.cc b/motorApp/LinMotSrc/LinMotRegister.cc new file mode 100644 index 000000000..79ac0bc66 --- /dev/null +++ b/motorApp/LinMotSrc/LinMotRegister.cc @@ -0,0 +1,52 @@ +/***************************************************************** + COPYRIGHT NOTIFICATION +***************************************************************** + +(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO + +This software was developed under a United States Government license +described on the COPYRIGHT_UniversityOfChicago file included as part +of this distribution. +**********************************************************************/ + +#include +#include "drvLinMot.h" +#include "epicsExport.h" + +extern "C" +{ + +// LinMotSetup arguments +static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; +static const iocshArg setupArg1 = {"Polling rate", iocshArgInt}; + +// LinMotConfig arguments +static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; +static const iocshArg configArg1 = {"asyn port name", iocshArgString}; +static const iocshArg configArg2 = {"Number of axes", iocshArgInt}; + +static const iocshArg * const LinMotSetupArgs[2] = {&setupArg0, &setupArg1}; +static const iocshArg * const LinMotConfigArgs[3] = {&configArg0, &configArg1, + &configArg2}; + +static const iocshFuncDef setupLinMot = {"LinMotSetup", 2, LinMotSetupArgs}; +static const iocshFuncDef configLinMot = {"LinMotConfig", 3, LinMotConfigArgs}; + +static void setupLinMotCallFunc(const iocshArgBuf *args) +{ + LinMotSetup(args[0].ival, args[1].ival); +} +static void configLinMotCallFunc(const iocshArgBuf *args) +{ + LinMotConfig(args[0].ival, args[1].sval, args[2].ival); +} + +static void LinMotRegister(void) +{ + iocshRegister(&setupLinMot, setupLinMotCallFunc); + iocshRegister(&configLinMot, configLinMotCallFunc); +} + +epicsExportRegistrar(LinMotRegister); + +} // extern "C" diff --git a/motorApp/LinMotSrc/Makefile b/motorApp/LinMotSrc/Makefile index 3a834c9e3..eb1d5ab92 100644 --- a/motorApp/LinMotSrc/Makefile +++ b/motorApp/LinMotSrc/Makefile @@ -2,18 +2,23 @@ TOP = ../.. include $(TOP)/configure/CONFIG -# The following are used for debugging messages. -#!USR_CXXFLAGS += -DDEBUG +# Both the following line, and a line in the *.dbd file, +# must be uncommented to use diagnostic debugging messages. +USR_CXXFLAGS += -DDEBUG DBD += devLinMotMotor.dbd -LIBRARY_IOC = LinMotMotor +LIBRARY_IOC = LinMot -# Khozu SC-800 motor controller. +# Intelligent Motion Systems driver support. +SRCS += LinMotRegister.cc SRCS += devLinMot.cc drvLinMot.cc -LinMotMotor_LIBS += motor asyn -LinMotMotor_LIBS += $(EPICS_BASE_IOC_LIBS) +LinMot_LIBS += motor +LinMot_LIBS += asyn +LinMot_LIBS += seq +LinMot_LIBS += pv +LinMot_LIBS += $(EPICS_BASE_IOC_LIBS) include $(TOP)/configure/RULES diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 98e443251..3a3653143 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -1,20 +1,34 @@ -#include -#include -#include "motorRecord.h" -#include "motor.h" -#include "motordevCom.h" -#include "drvLinMot.h" -#include "epicsExport.h" +#define VERSION 3.00 + + +#include +#include "motorRecord.h" +#include "motor.h" +#include "motordevCom.h" +#include "drvLinMot.h" +#include "epicsExport.h" + +#define STATIC static extern struct driver_table LinMot_access; +#define NINT(f) (long)((f)>0 ? (f)+0.5 : (f)-0.5) + +/*----------------debugging-----------------*/ +volatile int devLinMotDebug = 0; +extern "C" {epicsExportAddress(int, devLinMotDebug);} + +static inline void Debug(int level, const char *format, ...) { +} + /* ----------------Create the dsets for devLinMot----------------- */ -static struct driver_table *drvtabptr; -static long LinMot_init(void *); -static long LinMot_init_record(void *); -static long LinMot_start_trans(struct motorRecord *); -static RTN_STATUS LinMot_build_trans(motor_cmnd, double *, struct motorRecord *); -static RTN_STATUS LinMot_end_trans(struct motorRecord *); +STATIC struct driver_table *drvtabptr; +STATIC long LinMot_init(void *); +STATIC long LinMot_init_record(void *); +STATIC long LinMot_start_trans(struct motorRecord *); +STATIC RTN_STATUS LinMot_build_trans(motor_cmnd, double *, struct motorRecord *); +STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *); +STATIC long VELO = 0; struct motor_dset devLinMot = { @@ -28,9 +42,7 @@ struct motor_dset devLinMot = extern "C" {epicsExportAddress(dset,devLinMot);} /* --------------------------- program data --------------------- */ - /* This table is used to define the command types */ -/* WARNING! this must match "motor_cmnd" in motor.h */ static msg_types LinMot_table[] = { MOTION, /* MOVE_ABS */ @@ -53,59 +65,44 @@ static msg_types LinMot_table[] = { IMMEDIATE, /* DISABL_TORQUE */ IMMEDIATE, /* PRIMITIVE */ IMMEDIATE, /* SET_HIGH_LIMIT */ - IMMEDIATE, /* SET_LOW_LIMIT */ - VELOCITY, /* JOG_VELOCITY */ - IMMEDIATE /* SET_RESOLUTION */ + IMMEDIATE /* SET_LOW_LIMIT */ }; -static inline void Debug(int level, const char *format, ...) { - #ifdef DEBUG - if (level < devLinMotDebug) { - va_list pVar; - va_start(pVar, format); - vprintf(format, pVar); - va_end(pVar); - } - #endif -} - static struct board_stat **LinMot_cards; /* --------------------------- program data --------------------- */ - /* initialize device support for LinMot stepper motor */ -static long LinMot_init(void *arg) +STATIC long LinMot_init(void *arg) { - return(OK); + return((long)0); } /* initialize a record instance */ -static long LinMot_init_record(void *arg) +STATIC long LinMot_init_record(void *arg) { - return(OK); + return((long)0); } /* start building a transaction */ -static long LinMot_start_trans(struct motorRecord *mr) +STATIC long LinMot_start_trans(struct motorRecord *mr) { - return(OK); + return((long)0); } /* end building a transaction */ -static RTN_STATUS LinMot_end_trans(struct motorRecord *mr) +STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *mr) { return(OK); + } - /* add a part to the transaction */ -static RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) +STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) { - return(OK); -} - + return (OK); +} \ No newline at end of file diff --git a/motorApp/LinMotSrc/devLinMotMotor.dbd b/motorApp/LinMotSrc/devLinMotMotor.dbd index 6de3f1f30..ad1d09f0e 100644 --- a/motorApp/LinMotSrc/devLinMotMotor.dbd +++ b/motorApp/LinMotSrc/devLinMotMotor.dbd @@ -1,5 +1,5 @@ # LinMot motor controller support. -device(motor,VME_IO, devLinMot, "LinMot motor") +device(motor,VME_IO, devLinMot, "LinMot") driver(drvLinMot) registrar(LinMotRegister) variable(drvLinMotDebug) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 867a0a985..c018783de 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -1,59 +1,54 @@ -#include -#include /* isascii functions */ -#include -#include -#include -#include -#include -#include -#include "motor.h" -#include "motorRecord.h" -#include "drvLinMot.h" -#include "asynOctetSyncIO.h" -#include "epicsExport.h" +#include +#include +#include +#include +#include +#include +#include "motor.h" +#include "drvLinMot.h" +#include "asynOctetSyncIO.h" +#include "epicsExport.h" -#define BUFF_SIZE 120 /* Maximum length of string to/from LinMot */ +#define STATIC static -#define TIMEOUT 3.0 /* Command timeout in sec. */ +#define TIMEOUT 2.0 /* Command timeout in sec */ -#define LINMOT_MAX_CARDS 4 +#define BUFF_SIZE 200 /* Maximum length of string to/from LinMot */ -/* Delay after START_MOTION before a status update is possible */ -#define MOTION_DELAY 0.1 +/* This is a temporary fix to introduce a delayed reading of the motor + * position after a move completes + */ +volatile double drvLinMotReadbackDelay = 0.; + +struct mess_queue +{ + struct mess_node *head; + struct mess_node *tail; +}; /*----------------debugging-----------------*/ volatile int drvLinMotDebug = 0; extern "C" {epicsExportAddress(int, drvLinMotDebug);} static inline void Debug(int level, const char *format, ...) { - if (level < drvLinMotDebug) { - va_list pVar; - va_start(pVar, format); - vprintf(format, pVar); - va_end(pVar); - } } -/* Local data required for every driver; see "motordrvComCode.h" */ -#include "motordrvComCode.h" - - -/* This is a temporary fix to introduce a delayed reading of the motor - * position after a move completes - */ -volatile double drvLinMotReadbackDelay = 0.; - int LinMot_num_cards = 0; +/* Local data required for every driver; see "motordrvComCode.h" */ +#include "motordrvComCode.h" + /*----------------functions-----------------*/ -static int recv_mess(int card, char *com, int flag); -static RTN_STATUS send_mess(int card, char const *, char *name); -static int set_status(int card, int signal); +STATIC int recv_mess(int card, char *buff, int len); +STATIC RTN_STATUS send_mess(int, const char *, char *); +STATIC int send_recv_mess(int card, const char *out, char *in); +STATIC void start_status(int card); +STATIC int set_status(int card, int signal); static long report(int level); static long init(); -static int motor_init(); -static void query_done(int, int, struct mess_node *); +STATIC int motor_init(); +STATIC void query_done(int, int, struct mess_node *); /*----------------functions-----------------*/ @@ -76,7 +71,7 @@ struct driver_table LinMot_access = recv_mess, set_status, query_done, - NULL, + start_status, &initialized, NULL }; @@ -84,410 +79,90 @@ struct driver_table LinMot_access = struct drvLinMot_drvet { long number; +#ifdef __cplusplus long (*report) (int); long (*init) (void); +#else + DRVSUPFUN report; + DRVSUPFUN init; +#endif } drvLinMot = {2, report, init}; extern "C" {epicsExportAddress(drvet, drvLinMot);} -static struct thread_args targs = {SCAN_RATE, &LinMot_access, MOTION_DELAY}; +STATIC struct thread_args targs = {SCAN_RATE, &LinMot_access, 0.0}; /********************************************************* * Print out driver status report *********************************************************/ static long report(int level) { - int card; - - if (LinMot_num_cards <=0) - printf("[WARNING] No LinMot controllers found\n"); - else - { - for (card = 0; card < LinMot_num_cards; card++) - if (motor_state[card]) - printf("LinMot controller %d, id: %s \n", - card, motor_state[card]->ident); - } return (0); } - static long init() { - if (LinMot_num_cards <= 0) - { - Debug(1, "LinMotSetup() is missing from startup script.\n"); - return (ERROR); - } - return (OK); + return ((long) 0); } - -static void query_done(int card, int axis, struct mess_node *nodeptr) +STATIC void query_done(int card, int axis, struct mess_node *nodeptr) { } -static int send_recv_mess(int card, const char *out, char *response) +/********************************************************* + * Read the status and position of all motors on a card + * start_status(int card) + * if card == -1 then start all cards + *********************************************************/ +STATIC void start_status(int card) { - char *p, *tok_save; - struct LinMotController *cntrl; - asynStatus status; - size_t nwrite=0, nread=0; - int eomReason; - char temp[BUFF_SIZE]; - - response[0] = '\0'; - - /* Check that card exists */ - if (!motor_state[card]) - { - epicsPrintf("send_recv_mess - invalid card #%d\n", card); - return (-1); - } - - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - - /* Device support can send us multiple commands separated with ';' - * characters. The LinMot cannot handle more than 1 command on a line - * so send them separately */ - strcpy(temp, out); - for (p = epicsStrtok_r(temp, ";", &tok_save); - ((p != NULL) && (strlen(p) != 0)); - p = epicsStrtok_r(NULL, ";", &tok_save)) { - Debug(2, "send_recv_mess: sending message to card %d, message=%s\n", card, p); - status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), - response, BUFF_SIZE, TIMEOUT, - &nwrite, &nread, &eomReason); - } - - if (nread == 0) response[0] = '\0'; - if (nread > 0) { - Debug(2, "send_recv_mess: card %d, response = \"%s\"\n", card, response); - } - if (nread == 0) { - Debug(1, "send_recv_mess: card %d ERROR: no response\n", card); - } - return(strlen(response)); + /* The LinMot cannot query status or positions of all axes with a + * single command. This needs to be done on an axis-by-axis basis, + * so this function does nothing + */ } - /************************************************************** - * Parse status and position strings for a card and signal + * Query position and status for an axis * set_status() ************************************************************/ -static int set_status(int card, int signal) +STATIC int set_status(int card, int signal) { return (0); } - -/*****************************************************/ -/* send a message to the LinMot board */ -/* send_mess() */ -/*****************************************************/ -static RTN_STATUS send_mess(int card, char const *com, char *name) +STATIC RTN_STATUS send_mess(int card, const char *com, char *name) { - char *p, *tok_save; - char response[BUFF_SIZE]; - char temp[BUFF_SIZE]; - struct LinMotController *cntrl; - size_t nwrite, nread; - int eomReason; - - /* Check that card exists */ - if (!motor_state[card]) - { - epicsPrintf("send_mess - invalid card #%d\n", card); - return(ERROR); - } - - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - - /* Device support can send us multiple commands separated with ';' - * characters. The LinMot cannot handle more than 1 command on a line - * so send them separately */ - strcpy(temp, com); - for (p = epicsStrtok_r(temp, ";", &tok_save); - ((p != NULL) && (strlen(p) != 0)); - p = epicsStrtok_r(NULL, ";", &tok_save)) { - Debug(2, "send_mess: sending message to card %d, message=%s\n", card, p); - pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, - BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); - Debug(2, "send_mess: card %d, response=%s\n", card, response); - } return(OK); } - -/* - * FUNCTION... recv_mess(int card, char *com, int flag) - * - * INPUT ARGUMENTS... - * card - controller card # (0,1,...). - * *com - caller's response buffer. - * flag | FLUSH = this flag is ignored - the receive buffer is flushed - * on every write (see write_mess()) - * LOGIC... - * IF controller card does not exist. - * ERROR RETURN. - * ENDIF - * NORMAL RETURN. - */ - -static int recv_mess(int card, char *com, int flag) +STATIC int recv_mess(int card, char *com, int flag) { - double timeout; - char *pos; - char temp[BUFF_SIZE]; - int flush; - asynStatus status; - size_t nread=0; - int eomReason; - struct LinMotController *cntrl; - - com[0] = '\0'; - /* Check that card exists */ - if (!motor_state[card]) - { - epicsPrintf("resv_mess - invalid card #%d\n", card); - return(-1); - } - - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - - if (flag == FLUSH) { - flush = 1; - timeout = 0; - } else { - flush = 0; - timeout = TIMEOUT; - } - if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser); - status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, - timeout, &nread, &eomReason); - - /* The response from the PM304 is terminated with CR/LF. Remove these */ - if (nread == 0) com[0] = '\0'; - if (nread > 0) { - Debug(2, "recv_mess: card %d, flag=%d, message = \"%s\"\n", card, flag, com); - } - if (nread == 0) { - if (flag != FLUSH) { - Debug(1, "recv_mess: card %d read ERROR: no response\n", card); - } else { - Debug(3, "recv_mess: card %d flush returned no characters\n", card); - } - } - return(strlen(com)); + return(0); } +STATIC int send_recv_mess(int card, const char *out, char *response) +{ + return(0); +} -/*****************************************************/ -/* Setup system configuration */ -/* LinMotSetup() */ -/*****************************************************/ RTN_STATUS -LinMotSetup(int num_cards, /* maximum number of controllers in system. */ - int scan_rate) /* polling rate - 1/60 sec units. */ +LinMotSetup(int num_cards, /* maximum number of controllers in system */ + int scan_rate) /* polling rate - 1/60 sec units */ { - if (num_cards < 1 || num_cards > LINMOT_MAX_CARDS) - LinMot_num_cards = LINMOT_MAX_CARDS; - else - LinMot_num_cards = num_cards; - - /* Set motor polling task rate */ - if (scan_rate >= 1 && scan_rate <= 60) - targs.motor_scan_rate = scan_rate; - else - targs.motor_scan_rate = SCAN_RATE; - - /* - * Allocate space for motor_state structure pointers. Note this must be done - * before LinMotConfig is called, so it cannot be done in motor_init() - * This means that we must allocate space for a card without knowing - * if it really exists, which is not a serious problem since this is just - * an array of pointers. - */ - motor_state = (struct controller **) malloc(LinMot_num_cards * - sizeof(struct controller *)); - - for (int card = 0; card < LinMot_num_cards; card++) - motor_state[card] = (struct controller *) NULL; - - Debug(3, "LinMotSetup completed successfully\n"); return(OK); } - -/*****************************************************/ -/* Configure a controller */ -/* LinMotConfig() */ -/*****************************************************/ RTN_STATUS -LinMotConfig(int card, /* card being configured */ +LinMotConfig(int card, /* card being configured */ const char *port, /* asyn port name */ - int n_axes /* Number of axes */ - ) + int n_axes) { - struct LinMotController *cntrl; - if (card < 0 || card >= LinMot_num_cards) - return (ERROR); - - motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); - motor_state[card]->DevicePrivate = malloc(sizeof(struct LinMotController)); - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - cntrl->n_axes = n_axes; - strcpy(cntrl->port, port); - Debug(3, "LinMotConfig completed successfully\n"); return(OK); } - - -/*****************************************************/ -/* initialize all software and hardware */ -/* This is called from the initialization routine in */ -/* device support. */ -/* motor_init() */ -/*****************************************************/ -static int motor_init() +STATIC int motor_init() { - struct controller *brdptr; - struct LinMotController *cntrl; - int card_index, motor_index; - char buff[BUFF_SIZE]; - char command[20]; - int total_axis = 0; - int status; - bool success_rtn; - - initialized = true; /* Indicate that driver is initialized. */ - - /* Check for setup */ - if (LinMot_num_cards <= 0) - { - Debug(1, "LinMotSetup() is missing from startup script.\n"); - return (ERROR); - } - - for (card_index = 0; card_index < LinMot_num_cards; card_index++) - { - if (!motor_state[card_index]) - continue; - - brdptr = motor_state[card_index]; - total_cards = card_index + 1; - cntrl = (struct LinMotController *) brdptr->DevicePrivate; - - /* Initialize communications channel */ - success_rtn = false; - - status = pasynOctetSyncIO->connect(cntrl->port, 0, &cntrl->pasynUser, NULL); - success_rtn = (status == asynSuccess); - Debug(1, "motor_init, return from pasynOctetSyncIO->connect for port %s = %d, pasynUser=%p\n", cntrl->port, success_rtn, cntrl->pasynUser); - - if (success_rtn == true) - { - int retry = 0; - - /* Send a message to the board, see if it exists */ - /* flush any junk at input port - should not be any data available */ - do { - recv_mess(card_index, buff, FLUSH); - } while (strlen(buff) != 0); - - do - { - send_recv_mess(card_index, "!GPA", buff); - retry++; - /* Return value is length of response string */ - } while(strlen(buff) == 0 && retry < 3); - } - - if (success_rtn == true && strlen(buff) > 0) - { - brdptr->localaddr = (char *) NULL; - brdptr->motor_in_motion = 0; - /* Leave bdptr->cmnd_response false because we read each response */ - /* in send_mess and send_recv_mess. */ - brdptr->cmnd_response = false; - - /* Don't turn on motor power, too dangerous */ - /* send_mess(i, "1RSES;", buff); */ - send_mess(card_index, "!SS1A;", 0); /* Stop motor */ - send_recv_mess(card_index, "!PVA;", buff); /* Read controller ID string */ - strncpy(brdptr->ident, buff, MAX_IDENT_LEN); - - total_axis = cntrl->n_axes; - brdptr->total_axis = total_axis; - for (motor_index = 0; motor_index < total_axis; motor_index++) - { - struct mess_info *motor_info = &brdptr->motor_info[motor_index]; - - motor_info->motor_motion = NULL; - motor_info->status.All = 0; - motor_info->no_motion_count = 0; - motor_info->encoder_position = 0; - motor_info->position = 0; - set_status(card_index, motor_index); /* Read status of each motor */ - } - - } - else - motor_state[card_index] = (struct controller *) NULL; - } - - any_motor_in_motion = 0; - - Debug(3, "motor_init: spawning motor task\n"); - - mess_queue.head = (struct mess_node *) NULL; - mess_queue.tail = (struct mess_node *) NULL; - - free_list.head = (struct mess_node *) NULL; - free_list.tail = (struct mess_node *) NULL; - - epicsThreadCreate((char *) "tLinMot", epicsThreadPriorityMedium, - epicsThreadGetStackSize(epicsThreadStackMedium), - (EPICSTHREADFUNC) motor_task, (void *) &targs); - return(OK); } - -extern "C" -{ - -// Setup arguments - static const iocshArg setupArg0 = {"Maximum # of cards", iocshArgInt}; - static const iocshArg setupArg1 = {"Polling rate (HZ)", iocshArgInt}; -// Config arguments - static const iocshArg configArg0 = {"Card# being configured", iocshArgInt}; - static const iocshArg configArg1 = {"asyn port name", iocshArgString}; - static const iocshArg configArg2 = {"asyn address (GPIB)", iocshArgInt}; - - static const iocshArg *const SetupArgs[2] = {&setupArg0, &setupArg1}; - static const iocshArg *const ConfigArgs[3] = {&configArg0, &configArg1, &configArg2}; - - static const iocshFuncDef setupLinMot = {"LinMotSetup", 2, SetupArgs}; - static const iocshFuncDef configLinMot = {"LinMotConfig", 3, ConfigArgs}; - - static void setupLinMotCallFunc(const iocshArgBuf *args) - { - LinMotSetup(args[0].ival, args[1].ival); - } - static void configLinMotCallFunc (const iocshArgBuf *args) - { - LinMotConfig(args[0].ival, args[1].sval, args[2].ival); - } - - static void LinMotRegister(void) - { - iocshRegister(&setupLinMot, setupLinMotCallFunc); - iocshRegister(&configLinMot, configLinMotCallFunc); - } - - epicsExportRegistrar(LinMotRegister); - -} // extern "C" - diff --git a/motorApp/LinMotSrc/drvLinMot.h b/motorApp/LinMotSrc/drvLinMot.h index 4faf62fcf..10d73edd6 100644 --- a/motorApp/LinMotSrc/drvLinMot.h +++ b/motorApp/LinMotSrc/drvLinMot.h @@ -1,27 +1,46 @@ +/* File: drvLinMot.h */ +/* Version: 2.0 */ +/* Date Last Modified: 09-29-99 */ + + +/* Device Driver Support definitions for motor */ +/* + * Original Author: Mark Rivers + * Current Author: Mark Rivers + * Date: 11/20/98 + * + * Modification Log: + * ----------------- + * .01 11/20/98 mlr initialized from drvMM4000.h + * .02 09/29/99 mlr re-wrote for new version of motor software (V4) + * .03 02/11/03 mlr Added support for PM600 model + */ + #ifndef INCdrvLinMoth #define INCdrvLinMoth 1 -#include "motor.h" #include "motordrvCom.h" -#include "asynDriver.h" #include "asynOctetSyncIO.h" -#define LinMot_MAX_MOTORS 4 -#define LinMot_MSG_SIZE 80 -#define LinMot_STATUS_RETRY 10 +/* LinMot default profile. */ -/* End-of-string defines */ -#define LinMot_OUT_EOS "\r\n" /* Command */ -#define LinMot_IN_EOS "\r\n" /* Reply */ +#define LinMot_NUM_CARDS 4 +#define LinMot_MAX_CHANNELS 10 + +#define MODEL_LinMot 0 +#define MODEL_PM600 1 -/* Motion Master specific data is stored in this structure. */ struct LinMotController { - asynUser *pasynUser; /* For RS-232 */ + asynUser *pasynUser; /* asyn */ + int n_axes; /* Number of axes on this controller */ + int model; /* Model = MODEL_LinMot or MODEL_PM600 */ + int use_encoder[LinMot_MAX_CHANNELS]; /* Does axis have an encoder? */ + int home_mode[LinMot_MAX_CHANNELS]; /* The combined home modes for all axes */ char port[80]; /* asyn port name */ - int n_axes; /* Number of axes */ - int max_speed[LinMot_MAX_MOTORS]; /* steps/sec. */ }; -#endif /* INCdrvLinMoth */ +RTN_STATUS LinMotSetup(int, int); +RTN_STATUS LinMotConfig(int, const char *, int); +#endif /* INCdrvLinMoth */ From 113ece50e777c320759fbe40787ac8535870f82d Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 21 Mar 2017 10:55:55 +0000 Subject: [PATCH 059/232] Use straight up McLennan clone. No motor responses but IOC will start and send/recv messages --- motorApp/LinMotSrc/devLinMot.cc | 196 ++++++++++- motorApp/LinMotSrc/drvLinMot.cc | 562 +++++++++++++++++++++++++++++++- 2 files changed, 749 insertions(+), 9 deletions(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 3a3653143..55008fb39 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -19,8 +19,22 @@ volatile int devLinMotDebug = 0; extern "C" {epicsExportAddress(int, devLinMotDebug);} static inline void Debug(int level, const char *format, ...) { + #ifdef DEBUG + if (level < devLinMotDebug) { + va_list pVar; + va_start(pVar, format); + vprintf(format, pVar); + va_end(pVar); + } + #endif } + +/* Debugging levels: + * devLinMotDebug >= 3 Print new part of command and command string so far + * at the end of LinMot_build_trans + */ + /* ----------------Create the dsets for devLinMot----------------- */ STATIC struct driver_table *drvtabptr; STATIC long LinMot_init(void *); @@ -76,33 +90,205 @@ static struct board_stat **LinMot_cards; /* initialize device support for LinMot stepper motor */ STATIC long LinMot_init(void *arg) { - return((long)0); + long rtnval; + int after = (arg == 0) ? 0 : 1; + + if (after == 0) + { + drvtabptr = &LinMot_access; + (drvtabptr->init)(); + } + + rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &LinMot_cards); + return(rtnval); } /* initialize a record instance */ STATIC long LinMot_init_record(void *arg) { - return((long)0); + struct motorRecord *mr = (struct motorRecord *) arg; + long rtnval; + + rtnval = motor_init_record_com(mr, *drvtabptr->cardcnt_ptr, + drvtabptr, LinMot_cards); + return(rtnval); } /* start building a transaction */ STATIC long LinMot_start_trans(struct motorRecord *mr) { - return((long)0); + long rtnval; + rtnval = motor_start_trans_com(mr, LinMot_cards); + return(rtnval); } /* end building a transaction */ STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *mr) { - return(OK); + RTN_STATUS rtnval; + rtnval = motor_end_trans_com(mr, drvtabptr); + return(rtnval); } /* add a part to the transaction */ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) { - return (OK); + struct motor_trans *trans = (struct motor_trans *) mr->dpvt; + struct mess_node *motor_call; + struct controller *brdptr; + struct LinMotController *cntrl; + char buff[30]; + int axis, card; + RTN_STATUS rtnval; + double dval; + long ival; + + rtnval = OK; + buff[0] = '\0'; + /* Protect against NULL pointer with WRTITE_MSG(GO/STOP_AXIS/GET_INFO, NULL). */ + dval = (parms == NULL) ? 0.0 : *parms; + ival = NINT(dval); + + motor_call = &(trans->motor_call); + card = motor_call->card; + axis = motor_call->signal + 1; + brdptr = (*trans->tabptr->card_array)[card]; + if (brdptr == NULL) + return(rtnval = ERROR); + + cntrl = (struct LinMotController *) brdptr->DevicePrivate; + + if (LinMot_table[command] > motor_call->type) + motor_call->type = LinMot_table[command]; + + if (trans->state != BUILD_STATE) + return(rtnval = ERROR); + + if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0) + { + strcat(motor_call->message, mr->init); + strcat(motor_call->message, "\r"); + } + + switch (command) + { + case MOVE_ABS: + case MOVE_REL: + case HOME_FOR: + case HOME_REV: + case JOG: + if (strlen(mr->prem) != 0) + { + strcat(motor_call->message, mr->prem); + strcat(motor_call->message, ";"); + } + if (strlen(mr->post) != 0) + motor_call->postmsgptr = (char *) &mr->post; + break; + + default: + break; + } + + switch (command) + { + case MOVE_ABS: + sprintf(buff, "%dMA%ld;", axis, ival); + break; + case MOVE_REL: + sprintf(buff, "%dMR%ld;", axis, ival); + break; + case HOME_REV: + break; + case HOME_FOR: + break; + case LOAD_POS: + if (cntrl->use_encoder[axis-1]){ + sprintf(buff, "%dAP%ld;", axis, ival); + } + break; + case SET_VEL_BASE: + break; /* LinMot does not use base velocity */ + case SET_VELOCITY: + sprintf(buff, "%dSV%ld;", axis, ival); + VELO = ival; + break; + case SET_ACCEL: + sprintf(buff, "%dSA%ld;", axis, ival); + strcat(motor_call->message, buff); + sprintf(buff, "%dSD%ld;", axis, ival); + break; + case GO: + /* + * The LinMot starts moving immediately on move commands, GO command + * does nothing + */ + break; + case SET_ENC_RATIO: + /* + * The LinMot does not have the concept of encoder ratio, ignore this + * command + */ + break; + case GET_INFO: + /* These commands are not actually done by sending a message, but + rather they will indirectly cause the driver to read the status + of all motors */ + break; + case STOP_AXIS: + sprintf(buff, "%dST;", axis); + break; + case JOG: + if (cntrl->model == MODEL_LinMot) { + sprintf(buff, "%dSV%ld;", axis, ival); + strcat(motor_call->message, buff); + if (ival > 0) { + /* This is a positive move in LinMot coordinates */ + sprintf(buff, "%dCV1;", axis); + } else { + /* This is a negative move in LinMot coordinates */ + sprintf(buff, "%dCV-1;", axis); + } + } else { + sprintf(buff, "%dCV%ld;", axis, ival); + } + break; + case SET_PGAIN: + sprintf(buff, "%dKP%ld;", axis, ival); + break; + + case SET_IGAIN: + sprintf(buff, "%dKS%ld;", axis, ival); + break; + + case SET_DGAIN: + sprintf(buff, "%dKV%ld;", axis, ival); + break; + + case ENABLE_TORQUE: + sprintf(buff, "%dRS;", axis); + break; + + case DISABL_TORQUE: + sprintf(buff, "%dAB;", axis); + break; + + case SET_HIGH_LIMIT: + case SET_LOW_LIMIT: + trans->state = IDLE_STATE; /* No command sent to the controller. */ + /* The LinMot internal soft limits are very difficult to retrieve, not + * implemented yet */ + break; + + default: + rtnval = ERROR; + } + strcat(motor_call->message, buff); + Debug(3, "LinMot_build_trans: buff=%s, motor_call->message=%s\n", buff, motor_call->message); + + return (rtnval); } \ No newline at end of file diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index c018783de..743c0afbd 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -1,3 +1,36 @@ +/* File: drvLinMot.cc */ +/* Version: 2.01 */ +/* Date Last Modified: 10/26/99 */ + + +/* Device Driver Support routines for motor */ +/* + * Original Author: Mark Rivers + * Date: 11/20/98 + * + * Modification Log: + * ----------------- + * .01 11-20-98 mlr initialized from drvMM4000 + * .02 09-29-99 mlr Converted to motor record V4.04 + * .03 10-26-99 mlr Minor fixes for motor record V4.0 + * .04 08-16-00 mlr Fixed serious problem with limits - they were not + * correct, bring extract from wrong character in response + * Minor fixes to avoid compiler warnings + * .05 11-27-01 mlr Added global variable drvLinMotReadbackDelay. This is a + * double time in seconds to wait after the LinMot says the move + * is complete before reading the encoder position the final + * time. + * .06 07-03-2002 rls replaced RA_OVERTRAVEL with RA_PLUS_LS and RA_MINUS_LS + * .07 02-11-2003 mlr Added support for PM600 model. Added send_recv_mess which + * simplifies and cleans up. + * .08 03/27/03 rls R3.14 conversion. + * .09 02/03/04 rls Eliminate erroneous "Motor motion timeout ERROR". + * .10 04/20/04 mlr Convert from MPF to ASYN + * .11 07/16/04 rls removed unused Setup() argument. + * .12 09/20/04 rls support for 32axes/controller. + */ + + #include #include #include @@ -31,8 +64,24 @@ volatile int drvLinMotDebug = 0; extern "C" {epicsExportAddress(int, drvLinMotDebug);} static inline void Debug(int level, const char *format, ...) { + #ifdef DEBUG + if (level < drvLinMotDebug) { + va_list pVar; + va_start(pVar, format); + vprintf(format, pVar); + va_end(pVar); + } + #endif } +/* Debugging notes: + * drvLinMotDebug == 0 No debugging information is printed + * drvLinMotDebug >= 1 Warning information is printed + * drvLinMotDebug >= 2 Time-stamped messages are printed for each string + * sent to and received from the controller + * drvLinMotDebug >= 3 Additional debugging messages + */ + int LinMot_num_cards = 0; /* Local data required for every driver; see "motordrvComCode.h" */ @@ -92,16 +141,44 @@ extern "C" {epicsExportAddress(drvet, drvLinMot);} STATIC struct thread_args targs = {SCAN_RATE, &LinMot_access, 0.0}; + /********************************************************* * Print out driver status report *********************************************************/ static long report(int level) { + int card; + + if (LinMot_num_cards <=0) + printf(" NO LinMot controllers found\n"); + else + { + for (card = 0; card < LinMot_num_cards; card++) + if (motor_state[card]) + printf(" LinMot controller %d, id: %s \n", + card, + motor_state[card]->ident); + } return (0); } + static long init() { + /* + * We cannot call motor_init() here, because that function can do GPIB I/O, + * and hence requires that the drvGPIB have already been initialized. + * That cannot be guaranteed, so we need to call motor_init from device + * support + */ + /* Check for setup */ + if (LinMot_num_cards <= 0) + { + Debug(1, "init: *LinMot driver disabled*\n"); + Debug(1, "LinMotSetup() is missing from startup script.\n"); + return (ERROR); + } + return ((long) 0); } @@ -109,6 +186,7 @@ STATIC void query_done(int card, int axis, struct mess_node *nodeptr) { } + /********************************************************* * Read the status and position of all motors on a card * start_status(int card) @@ -122,6 +200,7 @@ STATIC void start_status(int card) */ } + /************************************************************** * Query position and status for an axis * set_status() @@ -129,40 +208,515 @@ STATIC void start_status(int card) STATIC int set_status(int card, int signal) { - return (0); + register struct mess_info *motor_info; + char command[BUFF_SIZE]; + char response[BUFF_SIZE]; + struct mess_node *nodeptr; + int rtn_state; + long motorData; + char buff[BUFF_SIZE]; + bool ls_active = false; + struct LinMotController *cntrl; + msta_field status; + + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + + motor_info = &(motor_state[card]->motor_info[signal]); + nodeptr = motor_info->motor_motion; + status.All = motor_info->status.All; + + /* Request the status of this motor */ + sprintf(command, "%dOS;", signal+1); + send_recv_mess(card, command, response); + Debug(2, "set_status, status query, card %d, response=%s\n", card, response); + + status.Bits.RA_PLUS_LS = 0; + status.Bits.RA_MINUS_LS = 0; + + if (cntrl->model == MODEL_LinMot) { + /* The response string is an eight character string of ones and zeroes */ + + if (response[3] == '0') { + status.Bits.RA_DONE = 0; + } else { + status.Bits.RA_DONE = 1; + if (drvLinMotReadbackDelay != 0.) + epicsThreadSleep(drvLinMotReadbackDelay); + } + + /* We used to check just response[2] for problem. + * However, it turns out that in firmware version 6.15 that bit=1 for no problem, + * but in 6.17 it is 0 for no problem! Check the last 4 bits individually instead. */ + status.Bits.RA_PROBLEM = 0; + if ((response[4] == '1') || + (response[5] == '1') || + (response[6] == '1') || + (response[7] == '1')) status.Bits.RA_PROBLEM = 1; + + if (response[1] == '1') { + status.Bits.RA_PLUS_LS = 1; + status.Bits.RA_DIRECTION = 1; + ls_active = true; + } + if (response[0] == '1') { + status.Bits.RA_MINUS_LS = 1; + status.Bits.RA_DIRECTION = 0; + ls_active = true; + } + } else { + /* The response string is 01: followed by an eight character string of ones and zeroes */ + strcpy(response, &response[3]); + if (response[0] == '0') + status.Bits.RA_DONE = 0; + else { + status.Bits.RA_DONE = 1; + if (drvLinMotReadbackDelay != 0.) + epicsThreadSleep(drvLinMotReadbackDelay); + } + + status.Bits.RA_PROBLEM = (response[1] == '1') ? 1 : 0; + + if (response[2] == '1') { + status.Bits.RA_PLUS_LS = 1; + } + if (response[3] == '1') { + status.Bits.RA_MINUS_LS = 1; + } + } + + + /* encoder status */ + status.Bits.EA_SLIP = 0; + status.Bits.EA_POSITION = 0; + status.Bits.EA_SLIP_STALL = 0; + status.Bits.EA_HOME = 0; + + /* Request the position of this motor */ + if (cntrl->use_encoder[signal]) { + sprintf(command, "%dOA;", signal+1); + } else { + sprintf(command, "%dOC;", signal+1); + } + send_recv_mess(card, command, response); + /* Parse the response string which is of the form "AP=10234" (LinMot) or 01:10234 (PM600)*/ + motorData = atoi(&response[3]); + Debug(2, "set_status, position query, card %d, response=%s\n", card, response); + + if (motorData == motor_info->position) + { + if (nodeptr != 0) /* Increment counter only if motor is moving. */ + motor_info->no_motion_count++; + } + else + { + status.Bits.RA_DIRECTION = (motorData >= motor_info->position) ? 1 : 0; + motor_info->position = motorData; + motor_info->encoder_position = motorData; + motor_info->no_motion_count = 0; + } + + /* Parse motor velocity? */ + /* NEEDS WORK */ + + motor_info->velocity = 0; + + if (!status.Bits.RA_DIRECTION) + motor_info->velocity *= -1; + + rtn_state = (!motor_info->no_motion_count || ls_active == true || + status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; + + /* Test for post-move string. */ + if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 && + nodeptr->postmsgptr != 0) + { + strcpy(buff, nodeptr->postmsgptr); + strcat(buff, "\r"); + send_mess(card, buff, (char) NULL); + nodeptr->postmsgptr = NULL; + } + + motor_info->status.All = status.All; + Debug(2, "set_status, return value=%d\n", rtn_state); + return (rtn_state); } + +/*****************************************************/ +/* send a message to the LinMot board */ +/* this function should be used when the LinMot */ +/* response string should be ignorred. */ +/* It reads the response string, since the LinMot */ +/* always sends one, but discards it. */ +/* Note that since it uses serialIOSendRecv it */ +/* flushes any remaining characters in the input */ +/* ring buffer */ +/* send_mess() */ +/*****************************************************/ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) { + char *p, *tok_save; + char response[BUFF_SIZE]; + char temp[BUFF_SIZE]; + struct LinMotController *cntrl; + size_t nwrite, nread; + int eomReason; + + /* Check that card exists */ + if (!motor_state[card]) + { + epicsPrintf("send_mess - invalid card #%d\n", card); + return(ERROR); + } + + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + + /* Device support can send us multiple commands separated with ';' + * characters. The LinMot cannot handle more than 1 command on a line + * so send them separately */ + strcpy(temp, com); + for (p = epicsStrtok_r(temp, ";", &tok_save); + ((p != NULL) && (strlen(p) != 0)); + p = epicsStrtok_r(NULL, ";", &tok_save)) { + Debug(2, "send_mess: sending message to card %d, message=%s\n", card, p); + pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, + BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); + Debug(2, "send_mess: card %d, response=%s\n", card, response); + } + return(OK); } + + +/*****************************************************/ +/* receive a message from the LinMot board */ +/* NOTE: This function is required by motordrvCom, */ +/* but it should not be used. Use send_recv_mess */ +/* instead, since it sends a receives a message as */ +/* atomic operation. */ +/* recv_mess() */ +/*****************************************************/ STATIC int recv_mess(int card, char *com, int flag) { - return(0); + double timeout; + char *pos; + char temp[BUFF_SIZE]; + int flush; + asynStatus status; + size_t nread=0; + int eomReason; + struct LinMotController *cntrl; + + com[0] = '\0'; + /* Check that card exists */ + if (!motor_state[card]) + { + epicsPrintf("resv_mess - invalid card #%d\n", card); + return(-1); + } + + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + + if (flag == FLUSH) { + flush = 1; + timeout = 0; + } else { + flush = 0; + timeout = TIMEOUT; + } + if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser); + status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, + timeout, &nread, &eomReason); + + /* The response from the LinMot is terminated with CR/LF. Remove these */ + if (nread == 0) com[0] = '\0'; + if (nread > 0) { + Debug(2, "recv_mess: card %d, flag=%d, message = \"%s\"\n", card, flag, com); + } + if (nread == 0) { + if (flag != FLUSH) { + Debug(1, "recv_mess: card %d read ERROR: no response\n", card); + } else { + Debug(3, "recv_mess: card %d flush returned no characters\n", card); + } + } + /* The PM600 always echoes the command sent to it, before sending the response. It is terminated + with a carriage return. So we need to delete all characters up to and including the first + carriage return */ + if (cntrl->model == MODEL_PM600) { + pos = strchr(com, '\r'); + if (pos != NULL) { + strcpy(temp, pos+1); + strcpy(com, temp); + } + } + return(strlen(com)); } + + +/*****************************************************/ +/* Send a message to the LinMot board and receive a */ +/* response as an atomic operation. */ +/* This function should be used when the LinMot */ +/* response string is required. */ +/* Note that since it uses serialIOSendRecv it */ +/* flushes any remaining characters in the input */ +/* ring buffer */ +/* send_recv_mess() */ +/*****************************************************/ STATIC int send_recv_mess(int card, const char *out, char *response) { - return(0); + char *p, *tok_save; + struct LinMotController *cntrl; + char *pos; + asynStatus status; + size_t nwrite=0, nread=0; + int eomReason; + char temp[BUFF_SIZE]; + + response[0] = '\0'; + + /* Check that card exists */ + if (!motor_state[card]) + { + epicsPrintf("send_recv_mess - invalid card #%d\n", card); + return (-1); + } + + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + + /* Device support can send us multiple commands separated with ';' + * characters. The LinMot cannot handle more than 1 command on a line + * so send them separately */ + strcpy(temp, out); + for (p = epicsStrtok_r(temp, ";", &tok_save); + ((p != NULL) && (strlen(p) != 0)); + p = epicsStrtok_r(NULL, ";", &tok_save)) { + Debug(2, "send_recv_mess: sending message to card %d, message=%s\n", card, p); + status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), + response, BUFF_SIZE, TIMEOUT, + &nwrite, &nread, &eomReason); + } + + /* The response from the LinMot is terminated with CR/LF. Remove these */ + if (nread == 0) response[0] = '\0';; + if (nread > 0) { + Debug(2, "send_recv_mess: card %d, response = \"%s\"\n", card, response); + } + if (nread == 0) { + Debug(1, "send_recv_mess: card %d ERROR: no response\n", card); + } + /* The PM600 always echoes the command sent to it, before sending the response. It is terminated + with a carriage return. So we need to delete all characters up to and including the first + carriage return */ + if (cntrl->model == MODEL_PM600) { + pos = strchr(response, '\r'); + if (pos != NULL) { + strcpy(temp, pos+1); + strcpy(response, temp); + } + } + return(strlen(response)); } + + +/*****************************************************/ +/* Setup system configuration */ +/* LinMotSetup() */ +/*****************************************************/ RTN_STATUS LinMotSetup(int num_cards, /* maximum number of controllers in system */ int scan_rate) /* polling rate - 1/60 sec units */ { + int itera; + + if (num_cards < 1 || num_cards > LinMot_NUM_CARDS) + LinMot_num_cards = LinMot_NUM_CARDS; + else + LinMot_num_cards = num_cards; + + /* Set motor polling task rate */ + if (scan_rate >= 1 && scan_rate <= 60) + targs.motor_scan_rate = scan_rate; + else + targs.motor_scan_rate = SCAN_RATE; + + /* + * Allocate space for motor_state structure pointers. Note this must be done + * before LinMotConfig is called, so it cannot be done in motor_init() + * This means that we must allocate space for a card without knowing + * if it really exists, which is not a serious problem since this is just + * an array of pointers. + */ + motor_state = (struct controller **) malloc(LinMot_num_cards * + sizeof(struct controller *)); + + for (itera = 0; itera < LinMot_num_cards; itera++) + motor_state[itera] = (struct controller *) NULL; return(OK); } + +/*****************************************************/ +/* Configure a controller */ +/* LinMotConfig() */ +/*****************************************************/ RTN_STATUS LinMotConfig(int card, /* card being configured */ const char *port, /* asyn port name */ - int n_axes) + int n_axes) /* Number of axes */ { + struct LinMotController *cntrl; + + if (card < 0 || card >= LinMot_num_cards) + return (ERROR); + + if (n_axes == 0) n_axes=1; /* This is a new parameter, some startup files don't have it yet */ + motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); + motor_state[card]->DevicePrivate = malloc(sizeof(struct LinMotController)); + cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; + cntrl->n_axes = n_axes; + cntrl->model = MODEL_LinMot; /* Assume LinMot initially */ + strcpy(cntrl->port, port); return(OK); } + + +/*****************************************************/ +/* initialize all software and hardware */ +/* This is called from the initialization routine in */ +/* device support. */ +/* motor_init() */ +/*****************************************************/ STATIC int motor_init() { + struct controller *brdptr; + struct LinMotController *cntrl; + int card_index, motor_index; + char buff[BUFF_SIZE]; + char command[20]; + int total_axis = 0; + int status; + bool success_rtn; + + initialized = true; /* Indicate that driver is initialized. */ + + /* Check for setup */ + if (LinMot_num_cards <= 0) + { + Debug(1, "motor_init: *LinMot driver disabled*\n"); + Debug(1, "LinMotSetup() is missing from startup script.\n"); + return (ERROR); + } + + for (card_index = 0; card_index < LinMot_num_cards; card_index++) + { + if (!motor_state[card_index]) + continue; + + brdptr = motor_state[card_index]; + total_cards = card_index + 1; + cntrl = (struct LinMotController *) brdptr->DevicePrivate; + + /* Initialize communications channel */ + success_rtn = false; + + status = pasynOctetSyncIO->connect(cntrl->port, 0, &cntrl->pasynUser, NULL); + success_rtn = (status == asynSuccess); + Debug(1, "motor_init, return from pasynOctetSyncIO->connect for port %s = %d, pasynUser=%p\n", cntrl->port, success_rtn, cntrl->pasynUser); + + if (success_rtn == true) + { + int retry = 0; + + /* Send a message to the board, see if it exists */ + /* flush any junk at input port - should not be any data available */ + do { + recv_mess(card_index, buff, FLUSH); + } while (strlen(buff) != 0); + + do + { + send_recv_mess(card_index, "1OA;", buff); + retry++; + /* Return value is length of response string */ + } while(strlen(buff) == 0 && retry < 3); + } + + if (success_rtn == true && strlen(buff) > 0) + { + brdptr->localaddr = (char *) NULL; + brdptr->motor_in_motion = 0; + /* Leave bdptr->cmnd_response false because we read each response */ + /* in send_mess and send_recv_mess. */ + brdptr->cmnd_response = false; + + /* Don't turn on motor power, too dangerous */ + /* send_mess(i, "1RSES;", buff); */ + send_mess(card_index, "1ST;", 0); /* Stop motor */ + send_recv_mess(card_index, "1ID;", buff); /* Read controller ID string */ + strncpy(brdptr->ident, buff, MAX_IDENT_LEN); + /* Parse the response to figure out what model this is */ + if (strstr(brdptr->ident, "LinMot") != NULL) { + cntrl->model = MODEL_LinMot; + } else { + cntrl->model = MODEL_PM600; + } + + total_axis = cntrl->n_axes; + brdptr->total_axis = total_axis; + start_status(card_index); + for (motor_index = 0; motor_index < total_axis; motor_index++) + { + struct mess_info *motor_info = &brdptr->motor_info[motor_index]; + + motor_info->motor_motion = NULL; + motor_info->status.All = 0; + motor_info->no_motion_count = 0; + motor_info->encoder_position = 0; + motor_info->position = 0; + /* Figure out if we have an encoder or not. If so use 0A, else use OC for readback. */ + sprintf(command, "%dID", motor_index+1); + send_recv_mess(card_index, command, buff); /* Read controller ID string for this axis */ + if (cntrl->model == MODEL_LinMot) { + /* For now always assume encoder if LinMot - needs work */ + cntrl->use_encoder[motor_index] = 1; + } else { + /* PM600 ident string identifies open loop stepper motors */ + if (strstr(buff, "Open loop stepper mode") != NULL) { + cntrl->use_encoder[motor_index] = 0; + } else { + cntrl->use_encoder[motor_index] = 1; + } + } + + Debug(3, "LinMot motor_init(), cntrl->model=%d, cntrl->use_encoder[%d]=%d.\n", cntrl->model, motor_index, cntrl->use_encoder[motor_index]); + + set_status(card_index, motor_index); /* Read status of each motor */ + } + + } + else + motor_state[card_index] = (struct controller *) NULL; + } + + any_motor_in_motion = 0; + + Debug(3, "motor_init: spawning motor task\n"); + + mess_queue.head = (struct mess_node *) NULL; + mess_queue.tail = (struct mess_node *) NULL; + + free_list.head = (struct mess_node *) NULL; + free_list.tail = (struct mess_node *) NULL; + + epicsThreadCreate((char *) "tLinMot", epicsThreadPriorityMedium, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) motor_task, (void *) &targs); + return(OK); } From 2ef78fa318e682a68c0b07b4c0b5209e22c85beb Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 21 Mar 2017 12:01:09 +0000 Subject: [PATCH 060/232] Remove unecessary controller fields --- motorApp/LinMotSrc/devLinMot.cc | 17 +--- motorApp/LinMotSrc/drvLinMot.cc | 145 +++++--------------------------- motorApp/LinMotSrc/drvLinMot.h | 3 - 3 files changed, 24 insertions(+), 141 deletions(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 55008fb39..a07066782 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -207,9 +207,6 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m case HOME_FOR: break; case LOAD_POS: - if (cntrl->use_encoder[axis-1]){ - sprintf(buff, "%dAP%ld;", axis, ival); - } break; case SET_VEL_BASE: break; /* LinMot does not use base velocity */ @@ -243,19 +240,7 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m sprintf(buff, "%dST;", axis); break; case JOG: - if (cntrl->model == MODEL_LinMot) { - sprintf(buff, "%dSV%ld;", axis, ival); - strcat(motor_call->message, buff); - if (ival > 0) { - /* This is a positive move in LinMot coordinates */ - sprintf(buff, "%dCV1;", axis); - } else { - /* This is a negative move in LinMot coordinates */ - sprintf(buff, "%dCV-1;", axis); - } - } else { - sprintf(buff, "%dCV%ld;", axis, ival); - } + sprintf(buff, "%dCV%ld;", axis, ival); break; case SET_PGAIN: sprintf(buff, "%dKP%ld;", axis, ival); diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 743c0afbd..af0638e37 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -1,36 +1,3 @@ -/* File: drvLinMot.cc */ -/* Version: 2.01 */ -/* Date Last Modified: 10/26/99 */ - - -/* Device Driver Support routines for motor */ -/* - * Original Author: Mark Rivers - * Date: 11/20/98 - * - * Modification Log: - * ----------------- - * .01 11-20-98 mlr initialized from drvMM4000 - * .02 09-29-99 mlr Converted to motor record V4.04 - * .03 10-26-99 mlr Minor fixes for motor record V4.0 - * .04 08-16-00 mlr Fixed serious problem with limits - they were not - * correct, bring extract from wrong character in response - * Minor fixes to avoid compiler warnings - * .05 11-27-01 mlr Added global variable drvLinMotReadbackDelay. This is a - * double time in seconds to wait after the LinMot says the move - * is complete before reading the encoder position the final - * time. - * .06 07-03-2002 rls replaced RA_OVERTRAVEL with RA_PLUS_LS and RA_MINUS_LS - * .07 02-11-2003 mlr Added support for PM600 model. Added send_recv_mess which - * simplifies and cleans up. - * .08 03/27/03 rls R3.14 conversion. - * .09 02/03/04 rls Eliminate erroneous "Motor motion timeout ERROR". - * .10 04/20/04 mlr Convert from MPF to ASYN - * .11 07/16/04 rls removed unused Setup() argument. - * .12 09/20/04 rls support for 32axes/controller. - */ - - #include #include #include @@ -233,58 +200,36 @@ STATIC int set_status(int card, int signal) status.Bits.RA_PLUS_LS = 0; status.Bits.RA_MINUS_LS = 0; - if (cntrl->model == MODEL_LinMot) { - /* The response string is an eight character string of ones and zeroes */ - - if (response[3] == '0') { - status.Bits.RA_DONE = 0; - } else { - status.Bits.RA_DONE = 1; - if (drvLinMotReadbackDelay != 0.) - epicsThreadSleep(drvLinMotReadbackDelay); - } - - /* We used to check just response[2] for problem. - * However, it turns out that in firmware version 6.15 that bit=1 for no problem, - * but in 6.17 it is 0 for no problem! Check the last 4 bits individually instead. */ - status.Bits.RA_PROBLEM = 0; - if ((response[4] == '1') || - (response[5] == '1') || - (response[6] == '1') || - (response[7] == '1')) status.Bits.RA_PROBLEM = 1; - - if (response[1] == '1') { + /* The response string is an eight character string of ones and zeroes */ + + if (response[3] == '0') { + status.Bits.RA_DONE = 0; + } else { + status.Bits.RA_DONE = 1; + if (drvLinMotReadbackDelay != 0.) + epicsThreadSleep(drvLinMotReadbackDelay); + } + + /* We used to check just response[2] for problem. + * However, it turns out that in firmware version 6.15 that bit=1 for no problem, + * but in 6.17 it is 0 for no problem! Check the last 4 bits individually instead. */ + status.Bits.RA_PROBLEM = 0; + if ((response[4] == '1') || + (response[5] == '1') || + (response[6] == '1') || + (response[7] == '1')) status.Bits.RA_PROBLEM = 1; + + if (response[1] == '1') { status.Bits.RA_PLUS_LS = 1; status.Bits.RA_DIRECTION = 1; ls_active = true; - } - if (response[0] == '1') { + } + if (response[0] == '1') { status.Bits.RA_MINUS_LS = 1; status.Bits.RA_DIRECTION = 0; ls_active = true; - } - } else { - /* The response string is 01: followed by an eight character string of ones and zeroes */ - strcpy(response, &response[3]); - if (response[0] == '0') - status.Bits.RA_DONE = 0; - else { - status.Bits.RA_DONE = 1; - if (drvLinMotReadbackDelay != 0.) - epicsThreadSleep(drvLinMotReadbackDelay); - } - - status.Bits.RA_PROBLEM = (response[1] == '1') ? 1 : 0; - - if (response[2] == '1') { - status.Bits.RA_PLUS_LS = 1; - } - if (response[3] == '1') { - status.Bits.RA_MINUS_LS = 1; - } } - /* encoder status */ status.Bits.EA_SLIP = 0; status.Bits.EA_POSITION = 0; @@ -292,11 +237,7 @@ STATIC int set_status(int card, int signal) status.Bits.EA_HOME = 0; /* Request the position of this motor */ - if (cntrl->use_encoder[signal]) { - sprintf(command, "%dOA;", signal+1); - } else { - sprintf(command, "%dOC;", signal+1); - } + sprintf(command, "%dOA;", signal+1); send_recv_mess(card, command, response); /* Parse the response string which is of the form "AP=10234" (LinMot) or 01:10234 (PM600)*/ motorData = atoi(&response[3]); @@ -441,16 +382,6 @@ STATIC int recv_mess(int card, char *com, int flag) Debug(3, "recv_mess: card %d flush returned no characters\n", card); } } - /* The PM600 always echoes the command sent to it, before sending the response. It is terminated - with a carriage return. So we need to delete all characters up to and including the first - carriage return */ - if (cntrl->model == MODEL_PM600) { - pos = strchr(com, '\r'); - if (pos != NULL) { - strcpy(temp, pos+1); - strcpy(com, temp); - } - } return(strlen(com)); } @@ -508,16 +439,6 @@ STATIC int send_recv_mess(int card, const char *out, char *response) if (nread == 0) { Debug(1, "send_recv_mess: card %d ERROR: no response\n", card); } - /* The PM600 always echoes the command sent to it, before sending the response. It is terminated - with a carriage return. So we need to delete all characters up to and including the first - carriage return */ - if (cntrl->model == MODEL_PM600) { - pos = strchr(response, '\r'); - if (pos != NULL) { - strcpy(temp, pos+1); - strcpy(response, temp); - } - } return(strlen(response)); } @@ -579,7 +500,6 @@ LinMotConfig(int card, /* card being configured */ motor_state[card]->DevicePrivate = malloc(sizeof(struct LinMotController)); cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; cntrl->n_axes = n_axes; - cntrl->model = MODEL_LinMot; /* Assume LinMot initially */ strcpy(cntrl->port, port); return(OK); } @@ -660,12 +580,6 @@ STATIC int motor_init() send_mess(card_index, "1ST;", 0); /* Stop motor */ send_recv_mess(card_index, "1ID;", buff); /* Read controller ID string */ strncpy(brdptr->ident, buff, MAX_IDENT_LEN); - /* Parse the response to figure out what model this is */ - if (strstr(brdptr->ident, "LinMot") != NULL) { - cntrl->model = MODEL_LinMot; - } else { - cntrl->model = MODEL_PM600; - } total_axis = cntrl->n_axes; brdptr->total_axis = total_axis; @@ -682,19 +596,6 @@ STATIC int motor_init() /* Figure out if we have an encoder or not. If so use 0A, else use OC for readback. */ sprintf(command, "%dID", motor_index+1); send_recv_mess(card_index, command, buff); /* Read controller ID string for this axis */ - if (cntrl->model == MODEL_LinMot) { - /* For now always assume encoder if LinMot - needs work */ - cntrl->use_encoder[motor_index] = 1; - } else { - /* PM600 ident string identifies open loop stepper motors */ - if (strstr(buff, "Open loop stepper mode") != NULL) { - cntrl->use_encoder[motor_index] = 0; - } else { - cntrl->use_encoder[motor_index] = 1; - } - } - - Debug(3, "LinMot motor_init(), cntrl->model=%d, cntrl->use_encoder[%d]=%d.\n", cntrl->model, motor_index, cntrl->use_encoder[motor_index]); set_status(card_index, motor_index); /* Read status of each motor */ } diff --git a/motorApp/LinMotSrc/drvLinMot.h b/motorApp/LinMotSrc/drvLinMot.h index 10d73edd6..b2ad08c84 100644 --- a/motorApp/LinMotSrc/drvLinMot.h +++ b/motorApp/LinMotSrc/drvLinMot.h @@ -34,9 +34,6 @@ struct LinMotController { asynUser *pasynUser; /* asyn */ int n_axes; /* Number of axes on this controller */ - int model; /* Model = MODEL_LinMot or MODEL_PM600 */ - int use_encoder[LinMot_MAX_CHANNELS]; /* Does axis have an encoder? */ - int home_mode[LinMot_MAX_CHANNELS]; /* The combined home modes for all axes */ char port[80]; /* asyn port name */ }; From 5d6f861b165474d9da0787c2f79aff75c2192a08 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 21 Mar 2017 14:54:18 +0000 Subject: [PATCH 061/232] Adjust commands to fit LinMot syntax --- motorApp/LinMotSrc/drvLinMot.cc | 147 ++++++++++---------------------- 1 file changed, 43 insertions(+), 104 deletions(-) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index af0638e37..900ef7d35 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -4,14 +4,16 @@ #include #include #include -#include "motor.h" -#include "drvLinMot.h" -#include "asynOctetSyncIO.h" +#include "motor.h" +#include "drvLinMot.h" +#include "asynOctetSyncIO.h" #include "epicsExport.h" #define STATIC static -#define TIMEOUT 2.0 /* Command timeout in sec */ +#define TIMEOUT 2.0 /* Command timeout in sec */ + +#define ASCII_0_TO_A 65 /* ASCII offset between 0 and A */ #define BUFF_SIZE 200 /* Maximum length of string to/from LinMot */ @@ -31,30 +33,19 @@ volatile int drvLinMotDebug = 0; extern "C" {epicsExportAddress(int, drvLinMotDebug);} static inline void Debug(int level, const char *format, ...) { - #ifdef DEBUG if (level < drvLinMotDebug) { va_list pVar; va_start(pVar, format); vprintf(format, pVar); va_end(pVar); } - #endif } -/* Debugging notes: - * drvLinMotDebug == 0 No debugging information is printed - * drvLinMotDebug >= 1 Warning information is printed - * drvLinMotDebug >= 2 Time-stamped messages are printed for each string - * sent to and received from the controller - * drvLinMotDebug >= 3 Additional debugging messages - */ - int LinMot_num_cards = 0; /* Local data required for every driver; see "motordrvComCode.h" */ #include "motordrvComCode.h" - /*----------------functions-----------------*/ STATIC int recv_mess(int card, char *buff, int len); STATIC RTN_STATUS send_mess(int, const char *, char *); @@ -108,71 +99,45 @@ extern "C" {epicsExportAddress(drvet, drvLinMot);} STATIC struct thread_args targs = {SCAN_RATE, &LinMot_access, 0.0}; - /********************************************************* * Print out driver status report *********************************************************/ static long report(int level) { - int card; - - if (LinMot_num_cards <=0) - printf(" NO LinMot controllers found\n"); - else - { - for (card = 0; card < LinMot_num_cards; card++) - if (motor_state[card]) - printf(" LinMot controller %d, id: %s \n", - card, - motor_state[card]->ident); + if (LinMot_num_cards <=0) { + epicsPrintf("LinMot report: No ontrollers found\n"); + } + else { + for (int card = 0; card < LinMot_num_cards; card++) + if (motor_state[card]) + printf("LinMot controller %d active\n", + card); } - return (0); + return (OK); } - static long init() { - /* - * We cannot call motor_init() here, because that function can do GPIB I/O, - * and hence requires that the drvGPIB have already been initialized. - * That cannot be guaranteed, so we need to call motor_init from device - * support - */ - /* Check for setup */ if (LinMot_num_cards <= 0) { - Debug(1, "init: *LinMot driver disabled*\n"); Debug(1, "LinMotSetup() is missing from startup script.\n"); return (ERROR); } - - return ((long) 0); + return (OK); } STATIC void query_done(int card, int axis, struct mess_node *nodeptr) { } - -/********************************************************* - * Read the status and position of all motors on a card - * start_status(int card) - * if card == -1 then start all cards - *********************************************************/ STATIC void start_status(int card) { - /* The LinMot cannot query status or positions of all axes with a - * single command. This needs to be done on an axis-by-axis basis, - * so this function does nothing - */ } - + /************************************************************** * Query position and status for an axis - * set_status() - ************************************************************/ - + **************************************************************/ STATIC int set_status(int card, int signal) { register struct mess_info *motor_info; @@ -193,14 +158,14 @@ STATIC int set_status(int card, int signal) status.All = motor_info->status.All; /* Request the status of this motor */ - sprintf(command, "%dOS;", signal+1); + sprintf(command, "!EW%c", signal+ASCII_0_TO_A); send_recv_mess(card, command, response); Debug(2, "set_status, status query, card %d, response=%s\n", card, response); status.Bits.RA_PLUS_LS = 0; status.Bits.RA_MINUS_LS = 0; - /* The response string is an eight character string of ones and zeroes */ + /* The response string is an integer representation of a bit sequence */ if (response[3] == '0') { status.Bits.RA_DONE = 0; @@ -214,7 +179,7 @@ STATIC int set_status(int card, int signal) * However, it turns out that in firmware version 6.15 that bit=1 for no problem, * but in 6.17 it is 0 for no problem! Check the last 4 bits individually instead. */ status.Bits.RA_PROBLEM = 0; - if ((response[4] == '1') || + /*if ((response[4] == '1') || (response[5] == '1') || (response[6] == '1') || (response[7] == '1')) status.Bits.RA_PROBLEM = 1; @@ -228,7 +193,7 @@ STATIC int set_status(int card, int signal) status.Bits.RA_MINUS_LS = 1; status.Bits.RA_DIRECTION = 0; ls_active = true; - } + }*/ /* encoder status */ status.Bits.EA_SLIP = 0; @@ -237,10 +202,10 @@ STATIC int set_status(int card, int signal) status.Bits.EA_HOME = 0; /* Request the position of this motor */ - sprintf(command, "%dOA;", signal+1); + sprintf(command, "!GP%c", signal+ASCII_0_TO_A); send_recv_mess(card, command, response); - /* Parse the response string which is of the form "AP=10234" (LinMot) or 01:10234 (PM600)*/ - motorData = atoi(&response[3]); + /* Parse the response string which is of the form "#1234" */ + motorData = atoi(&response[1]); Debug(2, "set_status, position query, card %d, response=%s\n", card, response); if (motorData == motor_info->position) @@ -282,12 +247,11 @@ STATIC int set_status(int card, int signal) return (rtn_state); } - /*****************************************************/ -/* send a message to the LinMot board */ -/* this function should be used when the LinMot */ -/* response string should be ignorred. */ -/* It reads the response string, since the LinMot */ +/* send a message to the LinMot board */ +/* this function should be used when the LinMot */ +/* response string should be ignored. */ +/* It reads the response string, since the LinMot */ /* always sends one, but discards it. */ /* Note that since it uses serialIOSendRecv it */ /* flushes any remaining characters in the input */ @@ -307,7 +271,7 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) if (!motor_state[card]) { epicsPrintf("send_mess - invalid card #%d\n", card); - return(ERROR); + return(ERROR); } cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; @@ -317,21 +281,18 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) * so send them separately */ strcpy(temp, com); for (p = epicsStrtok_r(temp, ";", &tok_save); - ((p != NULL) && (strlen(p) != 0)); - p = epicsStrtok_r(NULL, ";", &tok_save)) { + ((p != NULL) && (strlen(p) != 0)); + p = epicsStrtok_r(NULL, ";", &tok_save)) { Debug(2, "send_mess: sending message to card %d, message=%s\n", card, p); - pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, - BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); + pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, + BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); Debug(2, "send_mess: card %d, response=%s\n", card, response); } - return(OK); } - - /*****************************************************/ -/* receive a message from the LinMot board */ +/* receive a message from the LinMot board */ /* NOTE: This function is required by motordrvCom, */ /* but it should not be used. Use send_recv_mess */ /* instead, since it sends a receives a message as */ @@ -388,9 +349,9 @@ STATIC int recv_mess(int card, char *com, int flag) /*****************************************************/ -/* Send a message to the LinMot board and receive a */ +/* Send a message to the LinMot board and receive a */ /* response as an atomic operation. */ -/* This function should be used when the LinMot */ +/* This function should be used when the LinMot */ /* response string is required. */ /* Note that since it uses serialIOSendRecv it */ /* flushes any remaining characters in the input */ @@ -442,15 +403,9 @@ STATIC int send_recv_mess(int card, const char *out, char *response) return(strlen(response)); } - - -/*****************************************************/ -/* Setup system configuration */ -/* LinMotSetup() */ -/*****************************************************/ RTN_STATUS LinMotSetup(int num_cards, /* maximum number of controllers in system */ - int scan_rate) /* polling rate - 1/60 sec units */ + int scan_rate) /* polling rate - 1/60 sec units */ { int itera; @@ -480,15 +435,10 @@ LinMotSetup(int num_cards, /* maximum number of controllers in system */ return(OK); } - -/*****************************************************/ -/* Configure a controller */ -/* LinMotConfig() */ -/*****************************************************/ RTN_STATUS LinMotConfig(int card, /* card being configured */ - const char *port, /* asyn port name */ - int n_axes) /* Number of axes */ + const char *port, /* asyn port name */ + int n_axes) /* Number of axes */ { struct LinMotController *cntrl; @@ -505,7 +455,6 @@ LinMotConfig(int card, /* card being configured */ } - /*****************************************************/ /* initialize all software and hardware */ /* This is called from the initialization routine in */ @@ -561,7 +510,7 @@ STATIC int motor_init() do { - send_recv_mess(card_index, "1OA;", buff); + send_recv_mess(card_index, "!GPA", buff); retry++; /* Return value is length of response string */ } while(strlen(buff) == 0 && retry < 3); @@ -575,12 +524,6 @@ STATIC int motor_init() /* in send_mess and send_recv_mess. */ brdptr->cmnd_response = false; - /* Don't turn on motor power, too dangerous */ - /* send_mess(i, "1RSES;", buff); */ - send_mess(card_index, "1ST;", 0); /* Stop motor */ - send_recv_mess(card_index, "1ID;", buff); /* Read controller ID string */ - strncpy(brdptr->ident, buff, MAX_IDENT_LEN); - total_axis = cntrl->n_axes; brdptr->total_axis = total_axis; start_status(card_index); @@ -593,16 +536,12 @@ STATIC int motor_init() motor_info->no_motion_count = 0; motor_info->encoder_position = 0; motor_info->position = 0; - /* Figure out if we have an encoder or not. If so use 0A, else use OC for readback. */ - sprintf(command, "%dID", motor_index+1); - send_recv_mess(card_index, command, buff); /* Read controller ID string for this axis */ - set_status(card_index, motor_index); /* Read status of each motor */ } - } - else + else { motor_state[card_index] = (struct controller *) NULL; + } } any_motor_in_motion = 0; From 4f4eabf0b19ea2d2ff2d0213344dadd8d256ad45 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 21 Mar 2017 15:07:31 +0000 Subject: [PATCH 062/232] Get rid of initial # --- motorApp/LinMotSrc/drvLinMot.cc | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 900ef7d35..6bc84eb31 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -167,7 +167,7 @@ STATIC int set_status(int card, int signal) /* The response string is an integer representation of a bit sequence */ - if (response[3] == '0') { + if (atoi(&response[1]) == '0') { status.Bits.RA_DONE = 0; } else { status.Bits.RA_DONE = 1; @@ -334,6 +334,8 @@ STATIC int recv_mess(int card, char *com, int flag) /* The response from the LinMot is terminated with CR/LF. Remove these */ if (nread == 0) com[0] = '\0'; if (nread > 0) { + /* Get rid of the preliminary # */ + memmove (com, com+1, strlen (com+1) + 1); Debug(2, "recv_mess: card %d, flag=%d, message = \"%s\"\n", card, flag, com); } if (nread == 0) { @@ -395,6 +397,8 @@ STATIC int send_recv_mess(int card, const char *out, char *response) /* The response from the LinMot is terminated with CR/LF. Remove these */ if (nread == 0) response[0] = '\0';; if (nread > 0) { + /* Get rid of the preliminary # */ + memmove (response, response+1, strlen (response+1) + 1); Debug(2, "send_recv_mess: card %d, response = \"%s\"\n", card, response); } if (nread == 0) { From fc1806e598974f154aba8ca4a283b68a9b140464 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 21 Mar 2017 15:31:36 +0000 Subject: [PATCH 063/232] Translate to LinMot language status and problem queries --- motorApp/LinMotSrc/drvLinMot.cc | 59 ++++++++++++--------------------- 1 file changed, 22 insertions(+), 37 deletions(-) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 6bc84eb31..b8ae129a5 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -142,7 +142,7 @@ STATIC int set_status(int card, int signal) { register struct mess_info *motor_info; char command[BUFF_SIZE]; - char response[BUFF_SIZE]; + char warning_response[BUFF_SIZE], error_response[BUFF_SIZE], position_response[BUFF_SIZE]; struct mess_node *nodeptr; int rtn_state; long motorData; @@ -159,41 +159,30 @@ STATIC int set_status(int card, int signal) /* Request the status of this motor */ sprintf(command, "!EW%c", signal+ASCII_0_TO_A); - send_recv_mess(card, command, response); - Debug(2, "set_status, status query, card %d, response=%s\n", card, response); + send_recv_mess(card, command, warning_response); + Debug(2, "set_status, warning query, card %d, response=%s\n", card, warning_response); status.Bits.RA_PLUS_LS = 0; status.Bits.RA_MINUS_LS = 0; /* The response string is an integer representation of a bit sequence */ - if (atoi(&response[1]) == '0') { + int warning_code = atoi(warning_response); + int moving_bit = 9; + int moving = (warning_code & ( 1 << moving_bit )) >> moving_bit; + if (moving) { status.Bits.RA_DONE = 0; } else { status.Bits.RA_DONE = 1; if (drvLinMotReadbackDelay != 0.) epicsThreadSleep(drvLinMotReadbackDelay); } - - /* We used to check just response[2] for problem. - * However, it turns out that in firmware version 6.15 that bit=1 for no problem, - * but in 6.17 it is 0 for no problem! Check the last 4 bits individually instead. */ - status.Bits.RA_PROBLEM = 0; - /*if ((response[4] == '1') || - (response[5] == '1') || - (response[6] == '1') || - (response[7] == '1')) status.Bits.RA_PROBLEM = 1; - - if (response[1] == '1') { - status.Bits.RA_PLUS_LS = 1; - status.Bits.RA_DIRECTION = 1; - ls_active = true; - } - if (response[0] == '1') { - status.Bits.RA_MINUS_LS = 1; - status.Bits.RA_DIRECTION = 0; - ls_active = true; - }*/ + + /* Request the error state of the motor */ + sprintf(command, "!EE%c", signal+ASCII_0_TO_A); + send_recv_mess(card, command, error_response); + Debug(2, "set_status, warning query, card %d, response=%s\n", card, error_response); + status.Bits.RA_PROBLEM = atoi(error_response) > 0; /* encoder status */ status.Bits.EA_SLIP = 0; @@ -203,27 +192,23 @@ STATIC int set_status(int card, int signal) /* Request the position of this motor */ sprintf(command, "!GP%c", signal+ASCII_0_TO_A); - send_recv_mess(card, command, response); - /* Parse the response string which is of the form "#1234" */ - motorData = atoi(&response[1]); - Debug(2, "set_status, position query, card %d, response=%s\n", card, response); + send_recv_mess(card, command, position_response); + motorData = atoi(position_response); + Debug(2, "set_status, position query, card %d, response=%s\n", card, position_response); if (motorData == motor_info->position) { - if (nodeptr != 0) /* Increment counter only if motor is moving. */ - motor_info->no_motion_count++; + if (nodeptr != 0) /* Increment counter only if motor is moving. */ + motor_info->no_motion_count++; } else { - status.Bits.RA_DIRECTION = (motorData >= motor_info->position) ? 1 : 0; - motor_info->position = motorData; - motor_info->encoder_position = motorData; - motor_info->no_motion_count = 0; + status.Bits.RA_DIRECTION = (motorData >= motor_info->position) ? 1 : 0; + motor_info->position = motorData; + motor_info->encoder_position = motorData; + motor_info->no_motion_count = 0; } - /* Parse motor velocity? */ - /* NEEDS WORK */ - motor_info->velocity = 0; if (!status.Bits.RA_DIRECTION) From 30b26d888939533b76fddf34359a0b307304ab02 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 22 Mar 2017 08:49:03 +0000 Subject: [PATCH 064/232] Update move to use LinMot commands --- motorApp/LinMotSrc/devLinMot.cc | 101 ++++++++------------------------ motorApp/LinMotSrc/drvLinMot.cc | 19 ++---- 2 files changed, 31 insertions(+), 89 deletions(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index a07066782..ec050f4f8 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -1,15 +1,14 @@ -#define VERSION 3.00 - - #include -#include "motorRecord.h" -#include "motor.h" -#include "motordevCom.h" -#include "drvLinMot.h" +#include "motorRecord.h" +#include "motor.h" +#include "motordevCom.h" +#include "drvLinMot.h" #include "epicsExport.h" #define STATIC static +#define ASCII_0_TO_A 65 /* ASCII offset between 0 and A */ + extern struct driver_table LinMot_access; #define NINT(f) (long)((f)>0 ? (f)+0.5 : (f)-0.5) @@ -19,22 +18,14 @@ volatile int devLinMotDebug = 0; extern "C" {epicsExportAddress(int, devLinMotDebug);} static inline void Debug(int level, const char *format, ...) { - #ifdef DEBUG if (level < devLinMotDebug) { va_list pVar; va_start(pVar, format); vprintf(format, pVar); va_end(pVar); } - #endif } - -/* Debugging levels: - * devLinMotDebug >= 3 Print new part of command and command string so far - * at the end of LinMot_build_trans - */ - /* ----------------Create the dsets for devLinMot----------------- */ STATIC struct driver_table *drvtabptr; STATIC long LinMot_init(void *); @@ -42,7 +33,6 @@ STATIC long LinMot_init_record(void *); STATIC long LinMot_start_trans(struct motorRecord *); STATIC RTN_STATUS LinMot_build_trans(motor_cmnd, double *, struct motorRecord *); STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *); -STATIC long VELO = 0; struct motor_dset devLinMot = { @@ -137,12 +127,14 @@ STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *mr) /* add a part to the transaction */ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) { + Debug(2, "LinMot_build_trans\n"); struct motor_trans *trans = (struct motor_trans *) mr->dpvt; struct mess_node *motor_call; struct controller *brdptr; struct LinMotController *cntrl; char buff[30]; - int axis, card; + int card; + char axis; RTN_STATUS rtnval; double dval; long ival; @@ -155,7 +147,7 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m motor_call = &(trans->motor_call); card = motor_call->card; - axis = motor_call->signal + 1; + axis = motor_call->signal + ASCII_0_TO_A; brdptr = (*trans->tabptr->card_array)[card]; if (brdptr == NULL) return(rtnval = ERROR); @@ -197,83 +189,42 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m switch (command) { case MOVE_ABS: - sprintf(buff, "%dMA%ld;", axis, ival); - break; - case MOVE_REL: - sprintf(buff, "%dMR%ld;", axis, ival); - break; - case HOME_REV: - break; - case HOME_FOR: + sprintf(buff, "!SP%ld%c;", ival, axis); break; - case LOAD_POS: - break; - case SET_VEL_BASE: - break; /* LinMot does not use base velocity */ case SET_VELOCITY: - sprintf(buff, "%dSV%ld;", axis, ival); - VELO = ival; + sprintf(buff, "!SV%ld%c;", ival, axis); break; case SET_ACCEL: - sprintf(buff, "%dSA%ld;", axis, ival); - strcat(motor_call->message, buff); - sprintf(buff, "%dSD%ld;", axis, ival); - break; - case GO: - /* - * The LinMot starts moving immediately on move commands, GO command - * does nothing - */ - break; - case SET_ENC_RATIO: - /* - * The LinMot does not have the concept of encoder ratio, ignore this - * command - */ - break; - case GET_INFO: - /* These commands are not actually done by sending a message, but - rather they will indirectly cause the driver to read the status - of all motors */ - break; - case STOP_AXIS: - sprintf(buff, "%dST;", axis); - break; - case JOG: - sprintf(buff, "%dCV%ld;", axis, ival); + sprintf(buff, "!SA%ld%c;", ival, axis); break; case SET_PGAIN: - sprintf(buff, "%dKP%ld;", axis, ival); + sprintf(buff, "!DP%ld%c;", ival, axis); break; - case SET_IGAIN: - sprintf(buff, "%dKS%ld;", axis, ival); + sprintf(buff, "!DI%ld%c;", ival, axis); break; - case SET_DGAIN: - sprintf(buff, "%dKV%ld;", axis, ival); + sprintf(buff, "!DD%ld%c;", ival, axis); break; - + case GO: + case STOP_AXIS: + case SET_ENC_RATIO: + case GET_INFO: + case JOG: + case MOVE_REL: + case HOME_REV: + case HOME_FOR: + case LOAD_POS: + case SET_VEL_BASE: case ENABLE_TORQUE: - sprintf(buff, "%dRS;", axis); - break; - case DISABL_TORQUE: - sprintf(buff, "%dAB;", axis); - break; - case SET_HIGH_LIMIT: case SET_LOW_LIMIT: - trans->state = IDLE_STATE; /* No command sent to the controller. */ - /* The LinMot internal soft limits are very difficult to retrieve, not - * implemented yet */ break; - default: rtnval = ERROR; } strcat(motor_call->message, buff); Debug(3, "LinMot_build_trans: buff=%s, motor_call->message=%s\n", buff, motor_call->message); - return (rtnval); } \ No newline at end of file diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index b8ae129a5..1e52502d1 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -17,11 +17,6 @@ #define BUFF_SIZE 200 /* Maximum length of string to/from LinMot */ -/* This is a temporary fix to introduce a delayed reading of the motor - * position after a move completes - */ -volatile double drvLinMotReadbackDelay = 0.; - struct mess_queue { struct mess_node *head; @@ -169,14 +164,8 @@ STATIC int set_status(int card, int signal) int warning_code = atoi(warning_response); int moving_bit = 9; - int moving = (warning_code & ( 1 << moving_bit )) >> moving_bit; - if (moving) { - status.Bits.RA_DONE = 0; - } else { - status.Bits.RA_DONE = 1; - if (drvLinMotReadbackDelay != 0.) - epicsThreadSleep(drvLinMotReadbackDelay); - } + int moving = (warning_code & ( 1 << moving_bit )) >> moving_bit; + status.Bits.RA_DONE = !moving; /* Request the error state of the motor */ sprintf(command, "!EE%c", signal+ASCII_0_TO_A); @@ -209,10 +198,12 @@ STATIC int set_status(int card, int signal) motor_info->no_motion_count = 0; } + /* motor_info->velocity = 0; if (!status.Bits.RA_DIRECTION) motor_info->velocity *= -1; + */ rtn_state = (!motor_info->no_motion_count || ls_active == true || status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; @@ -384,7 +375,7 @@ STATIC int send_recv_mess(int card, const char *out, char *response) if (nread > 0) { /* Get rid of the preliminary # */ memmove (response, response+1, strlen (response+1) + 1); - Debug(2, "send_recv_mess: card %d, response = \"%s\"\n", card, response); + Debug(2, "send_recv_mess: card %d, response=%s\n", card, response); } if (nread == 0) { Debug(1, "send_recv_mess: card %d ERROR: no response\n", card); From 67547e97dbad26c9bdf16a664bed02981bfa9b0a Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 22 Mar 2017 09:45:53 +0000 Subject: [PATCH 065/232] Add some more debugging to check everything is as we expect --- motorApp/LinMotSrc/drvLinMot.cc | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 1e52502d1..b19dbeba4 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -165,13 +165,15 @@ STATIC int set_status(int card, int signal) int warning_code = atoi(warning_response); int moving_bit = 9; int moving = (warning_code & ( 1 << moving_bit )) >> moving_bit; - status.Bits.RA_DONE = !moving; + status.Bits.RA_DONE = moving ? 0 : 1; + Debug(2, "set_status, warning query, card %d, response=%d, moving=%d, done=%d\n", card, warning_code, moving, status.Bits.RA_DONE); /* Request the error state of the motor */ sprintf(command, "!EE%c", signal+ASCII_0_TO_A); send_recv_mess(card, command, error_response); - Debug(2, "set_status, warning query, card %d, response=%s\n", card, error_response); - status.Bits.RA_PROBLEM = atoi(error_response) > 0; + int error_code = atoi(error_response); + Debug(2, "set_status, error query, card %d, response=%d\n", card, error_code); + status.Bits.RA_PROBLEM = error_code > 0; /* encoder status */ status.Bits.EA_SLIP = 0; @@ -185,6 +187,7 @@ STATIC int set_status(int card, int signal) motorData = atoi(position_response); Debug(2, "set_status, position query, card %d, response=%s\n", card, position_response); + Debug(2, "set_status, position check, motorData %d, position %d\n", motorData, motor_info->position); if (motorData == motor_info->position) { if (nodeptr != 0) /* Increment counter only if motor is moving. */ @@ -193,10 +196,11 @@ STATIC int set_status(int card, int signal) else { status.Bits.RA_DIRECTION = (motorData >= motor_info->position) ? 1 : 0; - motor_info->position = motorData; - motor_info->encoder_position = motorData; - motor_info->no_motion_count = 0; + motor_info->position = motorData; + motor_info->encoder_position = motorData; + motor_info->no_motion_count = 0; } + Debug(2, "set_status, position check 2, motorData %d, position %d\n", motorData, motor_info->position); /* motor_info->velocity = 0; @@ -219,6 +223,7 @@ STATIC int set_status(int card, int signal) } motor_info->status.All = status.All; + Debug(2, "set_status, status, card %d, RA_DONE %d, RA_PROBLEM %d, RA_DIRECTION %d\n", card, status.Bits.RA_DONE, status.Bits.RA_PROBLEM, status.Bits.RA_DIRECTION); Debug(2, "set_status, return value=%d\n", rtn_state); return (rtn_state); } From 74845b2e889debb565803cc9b083357bca9fd391 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 22 Mar 2017 09:58:39 +0000 Subject: [PATCH 066/232] Add additional debug --- motorApp/LinMotSrc/devLinMot.cc | 11 ++++++++++- motorApp/LinMotSrc/drvLinMot.cc | 22 ++++++++++++++++++++++ 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index ec050f4f8..83f099c24 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -80,6 +80,8 @@ static struct board_stat **LinMot_cards; /* initialize device support for LinMot stepper motor */ STATIC long LinMot_init(void *arg) { + Debug(4, "LinMot_init...\n"); + long rtnval; int after = (arg == 0) ? 0 : 1; @@ -97,6 +99,8 @@ STATIC long LinMot_init(void *arg) /* initialize a record instance */ STATIC long LinMot_init_record(void *arg) { + Debug(4, "LinMot_init_record...\n"); + struct motorRecord *mr = (struct motorRecord *) arg; long rtnval; @@ -109,6 +113,8 @@ STATIC long LinMot_init_record(void *arg) /* start building a transaction */ STATIC long LinMot_start_trans(struct motorRecord *mr) { + Debug(4, "LinMot_start_trans...\n"); + long rtnval; rtnval = motor_start_trans_com(mr, LinMot_cards); return(rtnval); @@ -118,6 +124,8 @@ STATIC long LinMot_start_trans(struct motorRecord *mr) /* end building a transaction */ STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *mr) { + Debug(4, "LinMot_end_trans...\n"); + RTN_STATUS rtnval; rtnval = motor_end_trans_com(mr, drvtabptr); return(rtnval); @@ -127,7 +135,8 @@ STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *mr) /* add a part to the transaction */ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) { - Debug(2, "LinMot_build_trans\n"); + Debug(4, "LinMot_build_trans...\n"); + struct motor_trans *trans = (struct motor_trans *) mr->dpvt; struct mess_node *motor_call; struct controller *brdptr; diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index b19dbeba4..2f6b880d0 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -99,6 +99,8 @@ STATIC struct thread_args targs = {SCAN_RATE, &LinMot_access, 0.0}; *********************************************************/ static long report(int level) { + Debug(4, "report...\n"); + if (LinMot_num_cards <=0) { epicsPrintf("LinMot report: No ontrollers found\n"); } @@ -113,6 +115,8 @@ static long report(int level) static long init() { + Debug(4, "init...\n"); + if (LinMot_num_cards <= 0) { Debug(1, "LinMotSetup() is missing from startup script.\n"); @@ -123,10 +127,14 @@ static long init() STATIC void query_done(int card, int axis, struct mess_node *nodeptr) { + Debug(4, "query_done...\n"); + } STATIC void start_status(int card) { + Debug(4, "start_status...\n"); + } @@ -135,6 +143,8 @@ STATIC void start_status(int card) **************************************************************/ STATIC int set_status(int card, int signal) { + Debug(4, "set_status...\n"); + register struct mess_info *motor_info; char command[BUFF_SIZE]; char warning_response[BUFF_SIZE], error_response[BUFF_SIZE], position_response[BUFF_SIZE]; @@ -241,6 +251,8 @@ STATIC int set_status(int card, int signal) /*****************************************************/ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) { + Debug(4, "send_mess...\n"); + char *p, *tok_save; char response[BUFF_SIZE]; char temp[BUFF_SIZE]; @@ -282,6 +294,8 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) /*****************************************************/ STATIC int recv_mess(int card, char *com, int flag) { + Debug(4, "recv_mess...\n"); + double timeout; char *pos; char temp[BUFF_SIZE]; @@ -343,6 +357,8 @@ STATIC int recv_mess(int card, char *com, int flag) /*****************************************************/ STATIC int send_recv_mess(int card, const char *out, char *response) { + Debug(4, "send_recv_mess...\n"); + char *p, *tok_save; struct LinMotController *cntrl; char *pos; @@ -392,6 +408,8 @@ RTN_STATUS LinMotSetup(int num_cards, /* maximum number of controllers in system */ int scan_rate) /* polling rate - 1/60 sec units */ { + Debug(4, "LinMotSetup...\n"); + int itera; if (num_cards < 1 || num_cards > LinMot_NUM_CARDS) @@ -425,6 +443,8 @@ LinMotConfig(int card, /* card being configured */ const char *port, /* asyn port name */ int n_axes) /* Number of axes */ { + Debug(4, "LinMotConfig...\n"); + struct LinMotController *cntrl; if (card < 0 || card >= LinMot_num_cards) @@ -448,6 +468,8 @@ LinMotConfig(int card, /* card being configured */ /*****************************************************/ STATIC int motor_init() { + Debug(4, "motor_init...\n"); + struct controller *brdptr; struct LinMotController *cntrl; int card_index, motor_index; From ac458ae49fbf668d6a57664711dfc973721485dc Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 22 Mar 2017 11:23:39 +0000 Subject: [PATCH 067/232] Reuse logic for send/send_recv --- motorApp/LinMotSrc/devLinMot.cc | 10 ---- motorApp/LinMotSrc/drvLinMot.cc | 82 ++++++--------------------------- 2 files changed, 15 insertions(+), 77 deletions(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 83f099c24..262e08f31 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -80,8 +80,6 @@ static struct board_stat **LinMot_cards; /* initialize device support for LinMot stepper motor */ STATIC long LinMot_init(void *arg) { - Debug(4, "LinMot_init...\n"); - long rtnval; int after = (arg == 0) ? 0 : 1; @@ -99,8 +97,6 @@ STATIC long LinMot_init(void *arg) /* initialize a record instance */ STATIC long LinMot_init_record(void *arg) { - Debug(4, "LinMot_init_record...\n"); - struct motorRecord *mr = (struct motorRecord *) arg; long rtnval; @@ -113,8 +109,6 @@ STATIC long LinMot_init_record(void *arg) /* start building a transaction */ STATIC long LinMot_start_trans(struct motorRecord *mr) { - Debug(4, "LinMot_start_trans...\n"); - long rtnval; rtnval = motor_start_trans_com(mr, LinMot_cards); return(rtnval); @@ -124,8 +118,6 @@ STATIC long LinMot_start_trans(struct motorRecord *mr) /* end building a transaction */ STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *mr) { - Debug(4, "LinMot_end_trans...\n"); - RTN_STATUS rtnval; rtnval = motor_end_trans_com(mr, drvtabptr); return(rtnval); @@ -135,8 +127,6 @@ STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *mr) /* add a part to the transaction */ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) { - Debug(4, "LinMot_build_trans...\n"); - struct motor_trans *trans = (struct motor_trans *) mr->dpvt; struct mess_node *motor_call; struct controller *brdptr; diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 2f6b880d0..ad74e8458 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -11,7 +11,7 @@ #define STATIC static -#define TIMEOUT 2.0 /* Command timeout in sec */ +#define TIMEOUT 3.0 /* Command timeout in sec */ #define ASCII_0_TO_A 65 /* ASCII offset between 0 and A */ @@ -98,9 +98,7 @@ STATIC struct thread_args targs = {SCAN_RATE, &LinMot_access, 0.0}; * Print out driver status report *********************************************************/ static long report(int level) -{ - Debug(4, "report...\n"); - +{ if (LinMot_num_cards <=0) { epicsPrintf("LinMot report: No ontrollers found\n"); } @@ -114,9 +112,7 @@ static long report(int level) } static long init() -{ - Debug(4, "init...\n"); - +{ if (LinMot_num_cards <= 0) { Debug(1, "LinMotSetup() is missing from startup script.\n"); @@ -126,15 +122,11 @@ static long init() } STATIC void query_done(int card, int axis, struct mess_node *nodeptr) -{ - Debug(4, "query_done...\n"); - +{ } STATIC void start_status(int card) -{ - Debug(4, "start_status...\n"); - +{ } @@ -142,9 +134,8 @@ STATIC void start_status(int card) * Query position and status for an axis **************************************************************/ STATIC int set_status(int card, int signal) -{ - Debug(4, "set_status...\n"); - +{ + Debug(2, "set_status...\n"); register struct mess_info *motor_info; char command[BUFF_SIZE]; char warning_response[BUFF_SIZE], error_response[BUFF_SIZE], position_response[BUFF_SIZE]; @@ -196,8 +187,7 @@ STATIC int set_status(int card, int signal) send_recv_mess(card, command, position_response); motorData = atoi(position_response); Debug(2, "set_status, position query, card %d, response=%s\n", card, position_response); - - Debug(2, "set_status, position check, motorData %d, position %d\n", motorData, motor_info->position); + if (motorData == motor_info->position) { if (nodeptr != 0) /* Increment counter only if motor is moving. */ @@ -210,7 +200,6 @@ STATIC int set_status(int card, int signal) motor_info->encoder_position = motorData; motor_info->no_motion_count = 0; } - Debug(2, "set_status, position check 2, motorData %d, position %d\n", motorData, motor_info->position); /* motor_info->velocity = 0; @@ -233,55 +222,24 @@ STATIC int set_status(int card, int signal) } motor_info->status.All = status.All; - Debug(2, "set_status, status, card %d, RA_DONE %d, RA_PROBLEM %d, RA_DIRECTION %d\n", card, status.Bits.RA_DONE, status.Bits.RA_PROBLEM, status.Bits.RA_DIRECTION); Debug(2, "set_status, return value=%d\n", rtn_state); return (rtn_state); } /*****************************************************/ -/* send a message to the LinMot board */ -/* this function should be used when the LinMot */ +/* Send a message to the LinMot board. */ +/* This function should be used when the LinMot */ /* response string should be ignored. */ /* It reads the response string, since the LinMot */ /* always sends one, but discards it. */ /* Note that since it uses serialIOSendRecv it */ /* flushes any remaining characters in the input */ /* ring buffer */ -/* send_mess() */ /*****************************************************/ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) -{ - Debug(4, "send_mess...\n"); - - char *p, *tok_save; - char response[BUFF_SIZE]; +{ char temp[BUFF_SIZE]; - struct LinMotController *cntrl; - size_t nwrite, nread; - int eomReason; - - /* Check that card exists */ - if (!motor_state[card]) - { - epicsPrintf("send_mess - invalid card #%d\n", card); - return(ERROR); - } - - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - - /* Device support can send us multiple commands separated with ';' - * characters. The LinMot cannot handle more than 1 command on a line - * so send them separately */ - strcpy(temp, com); - for (p = epicsStrtok_r(temp, ";", &tok_save); - ((p != NULL) && (strlen(p) != 0)); - p = epicsStrtok_r(NULL, ";", &tok_save)) { - Debug(2, "send_mess: sending message to card %d, message=%s\n", card, p); - pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, - BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); - Debug(2, "send_mess: card %d, response=%s\n", card, response); - } - return(OK); + return (RTN_STATUS)send_recv_mess(card, com, temp); } /*****************************************************/ @@ -294,8 +252,6 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) /*****************************************************/ STATIC int recv_mess(int card, char *com, int flag) { - Debug(4, "recv_mess...\n"); - double timeout; char *pos; char temp[BUFF_SIZE]; @@ -357,8 +313,6 @@ STATIC int recv_mess(int card, char *com, int flag) /*****************************************************/ STATIC int send_recv_mess(int card, const char *out, char *response) { - Debug(4, "send_recv_mess...\n"); - char *p, *tok_save; struct LinMotController *cntrl; char *pos; @@ -386,16 +340,16 @@ STATIC int send_recv_mess(int card, const char *out, char *response) ((p != NULL) && (strlen(p) != 0)); p = epicsStrtok_r(NULL, ";", &tok_save)) { Debug(2, "send_recv_mess: sending message to card %d, message=%s\n", card, p); - status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), + status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); } /* The response from the LinMot is terminated with CR/LF. Remove these */ - if (nread == 0) response[0] = '\0';; + if (nread == 0) response[0] = '\0'; if (nread > 0) { /* Get rid of the preliminary # */ - memmove (response, response+1, strlen (response+1) + 1); + memmove(response, response+1, strlen (response+1) + 1); Debug(2, "send_recv_mess: card %d, response=%s\n", card, response); } if (nread == 0) { @@ -408,8 +362,6 @@ RTN_STATUS LinMotSetup(int num_cards, /* maximum number of controllers in system */ int scan_rate) /* polling rate - 1/60 sec units */ { - Debug(4, "LinMotSetup...\n"); - int itera; if (num_cards < 1 || num_cards > LinMot_NUM_CARDS) @@ -443,8 +395,6 @@ LinMotConfig(int card, /* card being configured */ const char *port, /* asyn port name */ int n_axes) /* Number of axes */ { - Debug(4, "LinMotConfig...\n"); - struct LinMotController *cntrl; if (card < 0 || card >= LinMot_num_cards) @@ -468,8 +418,6 @@ LinMotConfig(int card, /* card being configured */ /*****************************************************/ STATIC int motor_init() { - Debug(4, "motor_init...\n"); - struct controller *brdptr; struct LinMotController *cntrl; int card_index, motor_index; From e4aeaffad5a6184cdfc81cd2642a37d07f7eb82e Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 22 Mar 2017 13:07:35 +0000 Subject: [PATCH 068/232] Need load pos command for setting offsets --- motorApp/LinMotSrc/devLinMot.cc | 15 +++++++++++---- motorApp/LinMotSrc/drvLinMot.cc | 10 ++-------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 262e08f31..5779330c5 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -23,7 +23,7 @@ static inline void Debug(int level, const char *format, ...) { va_start(pVar, format); vprintf(format, pVar); va_end(pVar); - } + } } /* ----------------Create the dsets for devLinMot----------------- */ @@ -205,20 +205,27 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m case SET_DGAIN: sprintf(buff, "!DD%ld%c;", ival, axis); break; + case HOME_REV: + case HOME_FOR: + sprintf(buff, "!SR-1;"); + strcat(motor_call->message, buff); + sprintf(buff, "!SR+1;"); + break; + case LOAD_POS: + sprintf(buff, "!RP%ld%c;", ival, axis); + break; case GO: case STOP_AXIS: case SET_ENC_RATIO: case GET_INFO: case JOG: case MOVE_REL: - case HOME_REV: - case HOME_FOR: - case LOAD_POS: case SET_VEL_BASE: case ENABLE_TORQUE: case DISABL_TORQUE: case SET_HIGH_LIMIT: case SET_LOW_LIMIT: + Debug(3, "LinMot_build_trans: command=%d, %d, %d\n", command, GET_INFO, LOAD_POS); break; default: rtnval = ERROR; diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index ad74e8458..9d688c757 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -11,7 +11,7 @@ #define STATIC static -#define TIMEOUT 3.0 /* Command timeout in sec */ +#define TIMEOUT 5.0 /* Command timeout in sec */ #define ASCII_0_TO_A 65 /* ASCII offset between 0 and A */ @@ -135,7 +135,6 @@ STATIC void start_status(int card) **************************************************************/ STATIC int set_status(int card, int signal) { - Debug(2, "set_status...\n"); register struct mess_info *motor_info; char command[BUFF_SIZE]; char warning_response[BUFF_SIZE], error_response[BUFF_SIZE], position_response[BUFF_SIZE]; @@ -239,6 +238,7 @@ STATIC int set_status(int card, int signal) STATIC RTN_STATUS send_mess(int card, const char *com, char *name) { char temp[BUFF_SIZE]; + Debug(2, "send_mess: sending message via send_recv_mess to card %d, message=%s\n", card, com); return (RTN_STATUS)send_recv_mess(card, com, temp); } @@ -253,8 +253,6 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) STATIC int recv_mess(int card, char *com, int flag) { double timeout; - char *pos; - char temp[BUFF_SIZE]; int flush; asynStatus status; size_t nread=0; @@ -299,8 +297,6 @@ STATIC int recv_mess(int card, char *com, int flag) return(strlen(com)); } - - /*****************************************************/ /* Send a message to the LinMot board and receive a */ /* response as an atomic operation. */ @@ -315,7 +311,6 @@ STATIC int send_recv_mess(int card, const char *out, char *response) { char *p, *tok_save; struct LinMotController *cntrl; - char *pos; asynStatus status; size_t nwrite=0, nread=0; int eomReason; @@ -422,7 +417,6 @@ STATIC int motor_init() struct LinMotController *cntrl; int card_index, motor_index; char buff[BUFF_SIZE]; - char command[20]; int total_axis = 0; int status; bool success_rtn; From 7b68f85004bb12a894fe2ba6183e5fe9e0a9a72a Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 22 Mar 2017 13:40:58 +0000 Subject: [PATCH 069/232] Tidy up --- motorApp/LinMotSrc/devLinMot.cc | 11 ++--- motorApp/LinMotSrc/drvLinMot.cc | 77 ++++++++++----------------------- 2 files changed, 26 insertions(+), 62 deletions(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 5779330c5..6d17913d0 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -6,13 +6,11 @@ #include "epicsExport.h" #define STATIC static - #define ASCII_0_TO_A 65 /* ASCII offset between 0 and A */ +#define NINT(f) (long)((f)>0 ? (f)+0.5 : (f)-0.5) extern struct driver_table LinMot_access; -#define NINT(f) (long)((f)>0 ? (f)+0.5 : (f)-0.5) - /*----------------debugging-----------------*/ volatile int devLinMotDebug = 0; extern "C" {epicsExportAddress(int, devLinMotDebug);} @@ -72,7 +70,6 @@ static msg_types LinMot_table[] = { IMMEDIATE /* SET_LOW_LIMIT */ }; - static struct board_stat **LinMot_cards; /* --------------------------- program data --------------------- */ @@ -209,13 +206,14 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m case HOME_FOR: sprintf(buff, "!SR-1;"); strcat(motor_call->message, buff); - sprintf(buff, "!SR+1;"); break; case LOAD_POS: sprintf(buff, "!RP%ld%c;", ival, axis); break; - case GO: + /* These commands are unimplemented because there are no suitable motor commands + that fit them */ case STOP_AXIS: + case GO: case SET_ENC_RATIO: case GET_INFO: case JOG: @@ -225,7 +223,6 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m case DISABL_TORQUE: case SET_HIGH_LIMIT: case SET_LOW_LIMIT: - Debug(3, "LinMot_build_trans: command=%d, %d, %d\n", command, GET_INFO, LOAD_POS); break; default: rtnval = ERROR; diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 9d688c757..d314fdd33 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -10,11 +10,8 @@ #include "epicsExport.h" #define STATIC static - -#define TIMEOUT 5.0 /* Command timeout in sec */ - +#define TIMEOUT 3.0 /* Command timeout in sec */ #define ASCII_0_TO_A 65 /* ASCII offset between 0 and A */ - #define BUFF_SIZE 200 /* Maximum length of string to/from LinMot */ struct mess_queue @@ -39,7 +36,7 @@ static inline void Debug(int level, const char *format, ...) { int LinMot_num_cards = 0; /* Local data required for every driver; see "motordrvComCode.h" */ -#include "motordrvComCode.h" +#include "motordrvComCode.h" /*----------------functions-----------------*/ STATIC int recv_mess(int card, char *buff, int len); @@ -53,7 +50,6 @@ STATIC int motor_init(); STATIC void query_done(int, int, struct mess_node *); /*----------------functions-----------------*/ - struct driver_table LinMot_access = { motor_init, @@ -200,13 +196,6 @@ STATIC int set_status(int card, int signal) motor_info->no_motion_count = 0; } - /* - motor_info->velocity = 0; - - if (!status.Bits.RA_DIRECTION) - motor_info->velocity *= -1; - */ - rtn_state = (!motor_info->no_motion_count || ls_active == true || status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; @@ -225,16 +214,9 @@ STATIC int set_status(int card, int signal) return (rtn_state); } -/*****************************************************/ -/* Send a message to the LinMot board. */ -/* This function should be used when the LinMot */ -/* response string should be ignored. */ -/* It reads the response string, since the LinMot */ -/* always sends one, but discards it. */ -/* Note that since it uses serialIOSendRecv it */ -/* flushes any remaining characters in the input */ -/* ring buffer */ -/*****************************************************/ +/********************************************************* + * Send a message to the LinMot board. + *********************************************************/ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) { char temp[BUFF_SIZE]; @@ -242,14 +224,9 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) return (RTN_STATUS)send_recv_mess(card, com, temp); } -/*****************************************************/ -/* receive a message from the LinMot board */ -/* NOTE: This function is required by motordrvCom, */ -/* but it should not be used. Use send_recv_mess */ -/* instead, since it sends a receives a message as */ -/* atomic operation. */ -/* recv_mess() */ -/*****************************************************/ +/********************************************************* + * Receive a message from the LinMot board. + *********************************************************/ STATIC int recv_mess(int card, char *com, int flag) { double timeout; @@ -297,16 +274,9 @@ STATIC int recv_mess(int card, char *com, int flag) return(strlen(com)); } -/*****************************************************/ -/* Send a message to the LinMot board and receive a */ -/* response as an atomic operation. */ -/* This function should be used when the LinMot */ -/* response string is required. */ -/* Note that since it uses serialIOSendRecv it */ -/* flushes any remaining characters in the input */ -/* ring buffer */ -/* send_recv_mess() */ -/*****************************************************/ +/************************************************************ + * Send a message to the LinMot board and receive a resonse + ************************************************************/ STATIC int send_recv_mess(int card, const char *out, char *response) { char *p, *tok_save; @@ -353,9 +323,11 @@ STATIC int send_recv_mess(int card, const char *out, char *response) return(strlen(response)); } -RTN_STATUS -LinMotSetup(int num_cards, /* maximum number of controllers in system */ - int scan_rate) /* polling rate - 1/60 sec units */ +/************************************************************ + * Set up the LinMot motor + * Scan rat is in units of 1/60 seconds + ************************************************************/ +RTN_STATUS LinMotSetup(int num_cards, int scan_rate) { int itera; @@ -385,10 +357,9 @@ LinMotSetup(int num_cards, /* maximum number of controllers in system */ return(OK); } -RTN_STATUS -LinMotConfig(int card, /* card being configured */ - const char *port, /* asyn port name */ - int n_axes) /* Number of axes */ +RTN_STATUS LinMotConfig(int card, /* Card being configured */ + const char *port, /* Asyn port name */ + int n_axes) /* Number of axes */ { struct LinMotController *cntrl; @@ -404,13 +375,9 @@ LinMotConfig(int card, /* card being configured */ return(OK); } - -/*****************************************************/ -/* initialize all software and hardware */ -/* This is called from the initialization routine in */ -/* device support. */ -/* motor_init() */ -/*****************************************************/ +/************************************************************ + * Initialise motor software/hardware + ************************************************************/ STATIC int motor_init() { struct controller *brdptr; From ebd69a9d91138c14d7e3c8747e9c31c54a8a9887 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 22 Mar 2017 13:48:52 +0000 Subject: [PATCH 070/232] Reinitialization should not have been deleted --- motorApp/LinMotSrc/devLinMot.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 6d17913d0..25fe430da 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -206,6 +206,7 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m case HOME_FOR: sprintf(buff, "!SR-1;"); strcat(motor_call->message, buff); + sprintf(buff, "!SR+1;"); break; case LOAD_POS: sprintf(buff, "!RP%ld%c;", ival, axis); From 0feac01e9deb327edbc36bff9fdbb6bc81befee5 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 22 Mar 2017 14:06:17 +0000 Subject: [PATCH 071/232] Limit switches not supported --- motorApp/LinMotSrc/drvLinMot.cc | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index d314fdd33..57e5ea768 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -138,7 +138,6 @@ STATIC int set_status(int card, int signal) int rtn_state; long motorData; char buff[BUFF_SIZE]; - bool ls_active = false; struct LinMotController *cntrl; msta_field status; @@ -196,11 +195,11 @@ STATIC int set_status(int card, int signal) motor_info->no_motion_count = 0; } - rtn_state = (!motor_info->no_motion_count || ls_active == true || + rtn_state = (!motor_info->no_motion_count || status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; /* Test for post-move string. */ - if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 && + if (status.Bits.RA_DONE && nodeptr != 0 && nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); From 92633684e883beb6803af4ee59a4b1ed9f94b34c Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 4 Apr 2017 13:26:34 +0100 Subject: [PATCH 072/232] Add LinMot to list of motor sources --- motorApp/Makefile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/motorApp/Makefile b/motorApp/Makefile index 9f89a6532..ea6d381fe 100644 --- a/motorApp/Makefile +++ b/motorApp/Makefile @@ -39,6 +39,9 @@ AcsSrc_DEPEND_DIRS = MotorSrc DIRS += MclennanSrc MclennanSrc_DEPEND_DIRS = MotorSrc +DIRS += LinMotSrc +MclennanSrc_DEPEND_DIRS = MotorSrc + DIRS += PiSrc PiSrc_DEPEND_DIRS = MotorSrc From 43be9790bcfb827c90640eaaa0bed048ce49a6ff Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Mon, 10 Apr 2017 14:41:32 +0100 Subject: [PATCH 073/232] Add linmot motor record. Same as motor.db but offset can be set via macro --- motorApp/Db/Makefile | 1 + motorApp/Db/linmot_motor.db | 96 +++++++++++++++++++++++++++++++++++++ 2 files changed, 97 insertions(+) create mode 100644 motorApp/Db/linmot_motor.db diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index 2739dc11d..16728445c 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -16,6 +16,7 @@ include $(TOP)/configure/CONFIG # Create and install (or just install) # databases, templates, substitutions like this DB += motor.db +DB += linmot_motor.db DB += basic_motor.db DB += basic_asyn_motor.db DB += IMS_extra.db diff --git a/motorApp/Db/linmot_motor.db b/motorApp/Db/linmot_motor.db new file mode 100644 index 000000000..429cbc9dd --- /dev/null +++ b/motorApp/Db/linmot_motor.db @@ -0,0 +1,96 @@ +#! Generated by VisualDCT v2.6 +#! DBDSTART +#! DBDEND + + +record(motor, "$(P)$(M)") { + field(DESC, "$(DESC)") + field(DTYP, "$(DTYP)") + field(DIR, "$(DIR)") + field(VELO, "$(VELO)") + field(VBAS, "$(VBAS)") + field(ACCL, "$(ACCL)") + field(BDST, "$(BDST)") + field(BVEL, "$(BVEL)") + field(BACC, "$(BACC)") + field(OUT, "#C$(C) S$(S) @$(MOTORSIM=)") + field(MRES, "$(MRES)") + field(ERES, "$(ERES=)") + field(UEIP, "$(UEIP=0)") + field(PREC, "$(PREC)") + field(EGU, "$(EGU)") + field(DHLM, "$(DHLM)") + field(DLLM, "$(DLLM)") + field(INIT, "$(INIT)") + field(TWV, "1") + field(SDIS, "$(P)$(M)_able.VAL") +} + +# ISIS local aliases for genie_python +alias("$(P)$(M)", "$(P)$(M):SP") +alias("$(P)$(M)", "$(P)$(M):SP:RBV") + +record(bo, "$(P)$(M)_able") { + field(DESC, "motor enable") + field(PINI, "YES") + field(OUT, "$(P)$(M).DISP") + field(ZNAM, "Enable") + field(ONAM, "Disable") +} + +record(calcout, "$(P)$(M)_vCh") { + field(DESC, "change velocity") + field(CALC, "min(max(a*b,c),d)") + field(INPB, "$(P)$(M).S") + field(INPC, "$(P)$(M).SBAS") + field(INPD, "$(P)$(M).SMAX") + field(OUT, "$(P)$(M).S") +} + +record(calcout, "$(P)$(M)_twCh") { + field(DESC, "change TWV") + field(CALC, "min(max(a*b,c),d-e)") + field(INPB, "$(P)$(M).TWV") + field(INPC, "$(P)$(M).MRES") + field(INPD, "$(P)$(M).HLM") + field(INPE, "$(P)$(M).LLM") + field(OUT, "$(P)$(M).TWV") +} + +#! Further lines contain data used by VisualDCT +#! View(405,680,1.0) +#! Record("$(P)$(M)",840,674,0,0,"$(P)$(M)") +#! Field("$(P)$(M).DISP",16777215,1,"$(P)$(M).DISP") +#! Field("$(P)$(M).S",16777215,0,"$(P)$(M).S") +#! Field("$(P)$(M).SBAS",16777215,0,"$(P)$(M).SBAS") +#! Field("$(P)$(M).SMAX",16777215,0,"$(P)$(M).SMAX") +#! Field("$(P)$(M).TWV",16777215,0,"$(P)$(M).TWV") +#! Field("$(P)$(M).MRES",16777215,0,"$(P)$(M).MRES") +#! Field("$(P)$(M).HLM",16777215,0,"$(P)$(M).HLM") +#! Field("$(P)$(M).LLM",16777215,0,"$(P)$(M).LLM") +#! Field("$(P)$(M).SDIS",16777215,1,"$(P)$(M).SDIS") +#! Link("$(P)$(M).SDIS","$(P)$(M)_able.VAL") +#! Record("$(P)$(M)_able",1140,854,0,0,"$(P)$(M)_able") +#! Field("$(P)$(M)_able.OUT",16777215,0,"$(P)$(M)_able.OUT") +#! Link("$(P)$(M)_able.OUT","$(P)$(M).DISP") +#! Field("$(P)$(M)_able.VAL",16777215,0,"$(P)$(M)_able.VAL") +#! Record("$(P)$(M)_vCh",440,700,0,0,"$(P)$(M)_vCh") +#! Field("$(P)$(M)_vCh.OUT",16777215,1,"$(P)$(M)_vCh.OUT") +#! Link("$(P)$(M)_vCh.OUT","$(P)$(M).S") +#! Field("$(P)$(M)_vCh.INPB",16777215,1,"$(P)$(M)_vCh.INPB") +#! Link("$(P)$(M)_vCh.INPB","$(P)$(M).S") +#! Field("$(P)$(M)_vCh.INPC",16777215,1,"$(P)$(M)_vCh.INPC") +#! Link("$(P)$(M)_vCh.INPC","$(P)$(M).SBAS") +#! Field("$(P)$(M)_vCh.INPD",16777215,1,"$(P)$(M)_vCh.INPD") +#! Link("$(P)$(M)_vCh.INPD","$(P)$(M).SMAX") +#! Record("$(P)$(M)_twCh",440,926,0,0,"$(P)$(M)_twCh") +#! Field("$(P)$(M)_twCh.OUT",16777215,1,"$(P)$(M)_twCh.OUT") +#! Link("$(P)$(M)_twCh.OUT","$(P)$(M).TWV") +#! Field("$(P)$(M)_twCh.INPB",16777215,1,"$(P)$(M)_twCh.INPB") +#! Link("$(P)$(M)_twCh.INPB","$(P)$(M).TWV") +#! Field("$(P)$(M)_twCh.INPC",16777215,1,"$(P)$(M)_twCh.INPC") +#! Link("$(P)$(M)_twCh.INPC","$(P)$(M).MRES") +#! Field("$(P)$(M)_twCh.INPD",16777215,1,"$(P)$(M)_twCh.INPD") +#! Link("$(P)$(M)_twCh.INPD","$(P)$(M).HLM") +#! Field("$(P)$(M)_twCh.INPE",16777215,1,"$(P)$(M)_twCh.INPE") +#! Link("$(P)$(M)_twCh.INPE","$(P)$(M).LLM") From 47d57f80a8ae7e1863d87acff50fd132b1d8ac10 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Mon, 10 Apr 2017 14:49:52 +0100 Subject: [PATCH 074/232] Add extra linmot record separately rather than duplicating motor.db --- motorApp/Db/Makefile | 2 +- motorApp/Db/linmot_extra.db | 3 ++ motorApp/Db/linmot_motor.db | 96 ------------------------------------- 3 files changed, 4 insertions(+), 97 deletions(-) create mode 100644 motorApp/Db/linmot_extra.db delete mode 100644 motorApp/Db/linmot_motor.db diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index 16728445c..abbe6a02b 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -16,7 +16,6 @@ include $(TOP)/configure/CONFIG # Create and install (or just install) # databases, templates, substitutions like this DB += motor.db -DB += linmot_motor.db DB += basic_motor.db DB += basic_asyn_motor.db DB += IMS_extra.db @@ -46,6 +45,7 @@ DB += profileMoveControllerXPS.template DB += profileMoveAxisXPS.template DB += PI_Support.db PI_SupportCtrl.db DB += Phytron_motor.db Phytron_I1AM01.db Phytron_MCM01.db +DB += linmot_extra.db #---------------------------------------------------- # Declare template files which do not show up in DB diff --git a/motorApp/Db/linmot_extra.db b/motorApp/Db/linmot_extra.db new file mode 100644 index 000000000..a3ff94d13 --- /dev/null +++ b/motorApp/Db/linmot_extra.db @@ -0,0 +1,3 @@ +record(motor, "$(P)$(M)") { + field(OFF, "$(OFF)") +} \ No newline at end of file diff --git a/motorApp/Db/linmot_motor.db b/motorApp/Db/linmot_motor.db deleted file mode 100644 index 429cbc9dd..000000000 --- a/motorApp/Db/linmot_motor.db +++ /dev/null @@ -1,96 +0,0 @@ -#! Generated by VisualDCT v2.6 -#! DBDSTART -#! DBDEND - - -record(motor, "$(P)$(M)") { - field(DESC, "$(DESC)") - field(DTYP, "$(DTYP)") - field(DIR, "$(DIR)") - field(VELO, "$(VELO)") - field(VBAS, "$(VBAS)") - field(ACCL, "$(ACCL)") - field(BDST, "$(BDST)") - field(BVEL, "$(BVEL)") - field(BACC, "$(BACC)") - field(OUT, "#C$(C) S$(S) @$(MOTORSIM=)") - field(MRES, "$(MRES)") - field(ERES, "$(ERES=)") - field(UEIP, "$(UEIP=0)") - field(PREC, "$(PREC)") - field(EGU, "$(EGU)") - field(DHLM, "$(DHLM)") - field(DLLM, "$(DLLM)") - field(INIT, "$(INIT)") - field(TWV, "1") - field(SDIS, "$(P)$(M)_able.VAL") -} - -# ISIS local aliases for genie_python -alias("$(P)$(M)", "$(P)$(M):SP") -alias("$(P)$(M)", "$(P)$(M):SP:RBV") - -record(bo, "$(P)$(M)_able") { - field(DESC, "motor enable") - field(PINI, "YES") - field(OUT, "$(P)$(M).DISP") - field(ZNAM, "Enable") - field(ONAM, "Disable") -} - -record(calcout, "$(P)$(M)_vCh") { - field(DESC, "change velocity") - field(CALC, "min(max(a*b,c),d)") - field(INPB, "$(P)$(M).S") - field(INPC, "$(P)$(M).SBAS") - field(INPD, "$(P)$(M).SMAX") - field(OUT, "$(P)$(M).S") -} - -record(calcout, "$(P)$(M)_twCh") { - field(DESC, "change TWV") - field(CALC, "min(max(a*b,c),d-e)") - field(INPB, "$(P)$(M).TWV") - field(INPC, "$(P)$(M).MRES") - field(INPD, "$(P)$(M).HLM") - field(INPE, "$(P)$(M).LLM") - field(OUT, "$(P)$(M).TWV") -} - -#! Further lines contain data used by VisualDCT -#! View(405,680,1.0) -#! Record("$(P)$(M)",840,674,0,0,"$(P)$(M)") -#! Field("$(P)$(M).DISP",16777215,1,"$(P)$(M).DISP") -#! Field("$(P)$(M).S",16777215,0,"$(P)$(M).S") -#! Field("$(P)$(M).SBAS",16777215,0,"$(P)$(M).SBAS") -#! Field("$(P)$(M).SMAX",16777215,0,"$(P)$(M).SMAX") -#! Field("$(P)$(M).TWV",16777215,0,"$(P)$(M).TWV") -#! Field("$(P)$(M).MRES",16777215,0,"$(P)$(M).MRES") -#! Field("$(P)$(M).HLM",16777215,0,"$(P)$(M).HLM") -#! Field("$(P)$(M).LLM",16777215,0,"$(P)$(M).LLM") -#! Field("$(P)$(M).SDIS",16777215,1,"$(P)$(M).SDIS") -#! Link("$(P)$(M).SDIS","$(P)$(M)_able.VAL") -#! Record("$(P)$(M)_able",1140,854,0,0,"$(P)$(M)_able") -#! Field("$(P)$(M)_able.OUT",16777215,0,"$(P)$(M)_able.OUT") -#! Link("$(P)$(M)_able.OUT","$(P)$(M).DISP") -#! Field("$(P)$(M)_able.VAL",16777215,0,"$(P)$(M)_able.VAL") -#! Record("$(P)$(M)_vCh",440,700,0,0,"$(P)$(M)_vCh") -#! Field("$(P)$(M)_vCh.OUT",16777215,1,"$(P)$(M)_vCh.OUT") -#! Link("$(P)$(M)_vCh.OUT","$(P)$(M).S") -#! Field("$(P)$(M)_vCh.INPB",16777215,1,"$(P)$(M)_vCh.INPB") -#! Link("$(P)$(M)_vCh.INPB","$(P)$(M).S") -#! Field("$(P)$(M)_vCh.INPC",16777215,1,"$(P)$(M)_vCh.INPC") -#! Link("$(P)$(M)_vCh.INPC","$(P)$(M).SBAS") -#! Field("$(P)$(M)_vCh.INPD",16777215,1,"$(P)$(M)_vCh.INPD") -#! Link("$(P)$(M)_vCh.INPD","$(P)$(M).SMAX") -#! Record("$(P)$(M)_twCh",440,926,0,0,"$(P)$(M)_twCh") -#! Field("$(P)$(M)_twCh.OUT",16777215,1,"$(P)$(M)_twCh.OUT") -#! Link("$(P)$(M)_twCh.OUT","$(P)$(M).TWV") -#! Field("$(P)$(M)_twCh.INPB",16777215,1,"$(P)$(M)_twCh.INPB") -#! Link("$(P)$(M)_twCh.INPB","$(P)$(M).TWV") -#! Field("$(P)$(M)_twCh.INPC",16777215,1,"$(P)$(M)_twCh.INPC") -#! Link("$(P)$(M)_twCh.INPC","$(P)$(M).MRES") -#! Field("$(P)$(M)_twCh.INPD",16777215,1,"$(P)$(M)_twCh.INPD") -#! Link("$(P)$(M)_twCh.INPD","$(P)$(M).HLM") -#! Field("$(P)$(M)_twCh.INPE",16777215,1,"$(P)$(M)_twCh.INPE") -#! Link("$(P)$(M)_twCh.INPE","$(P)$(M).LLM") From 5b2825ef3865a634220d48aed7c163fcf71f1c51 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 11 Apr 2017 09:37:25 +0100 Subject: [PATCH 075/232] Speed has to be translated into motor units --- motorApp/LinMotSrc/devLinMot.cc | 6 ++++-- motorApp/LinMotSrc/drvLinMot.cc | 14 ++++++++------ motorApp/LinMotSrc/drvLinMot.h | 1 + 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 25fe430da..9bf72ff3b 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -16,7 +16,7 @@ volatile int devLinMotDebug = 0; extern "C" {epicsExportAddress(int, devLinMotDebug);} static inline void Debug(int level, const char *format, ...) { - if (level < devLinMotDebug) { + if (true) { va_list pVar; va_start(pVar, format); vprintf(format, pVar); @@ -188,7 +188,9 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m sprintf(buff, "!SP%ld%c;", ival, axis); break; case SET_VELOCITY: - sprintf(buff, "!SV%ld%c;", ival, axis); + /* Speed resolution in nm/s. Convert to mm/s. */ + Debug(2, "Speed %ld, %ld", ival, cntrl->speed_resolution); + sprintf(buff, "!SV%ld%c;", int(ival*1000000.0/cntrl->speed_resolution), axis); break; case SET_ACCEL: sprintf(buff, "!SA%ld%c;", ival, axis); diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 57e5ea768..cc3afab51 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -382,7 +382,7 @@ STATIC int motor_init() struct controller *brdptr; struct LinMotController *cntrl; int card_index, motor_index; - char buff[BUFF_SIZE]; + char response[BUFF_SIZE], command[BUFF_SIZE]; int total_axis = 0; int status; bool success_rtn; @@ -420,18 +420,19 @@ STATIC int motor_init() /* Send a message to the board, see if it exists */ /* flush any junk at input port - should not be any data available */ do { - recv_mess(card_index, buff, FLUSH); - } while (strlen(buff) != 0); + recv_mess(card_index, response, FLUSH); + } while (strlen(response) != 0); do { - send_recv_mess(card_index, "!GPA", buff); + sprintf(command, "!VI%c;", card_index + ASCII_0_TO_A); + send_recv_mess(card_index, command, response); retry++; /* Return value is length of response string */ - } while(strlen(buff) == 0 && retry < 3); + } while(strlen(response) == 0 && retry < 3); } - if (success_rtn == true && strlen(buff) > 0) + if (success_rtn == true && strlen(response) > 0) { brdptr->localaddr = (char *) NULL; brdptr->motor_in_motion = 0; @@ -439,6 +440,7 @@ STATIC int motor_init() /* in send_mess and send_recv_mess. */ brdptr->cmnd_response = false; + cntrl->speed_resolution = atoi(response); total_axis = cntrl->n_axes; brdptr->total_axis = total_axis; start_status(card_index); diff --git a/motorApp/LinMotSrc/drvLinMot.h b/motorApp/LinMotSrc/drvLinMot.h index b2ad08c84..0ce93b616 100644 --- a/motorApp/LinMotSrc/drvLinMot.h +++ b/motorApp/LinMotSrc/drvLinMot.h @@ -35,6 +35,7 @@ struct LinMotController asynUser *pasynUser; /* asyn */ int n_axes; /* Number of axes on this controller */ char port[80]; /* asyn port name */ + int speed_resolution; /* The motor speed resolution */ }; RTN_STATUS LinMotSetup(int, int); From 4fcbd5e562b37ea1065fc4cf8ab2fc1e796b8ab0 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 11 Apr 2017 09:38:21 +0100 Subject: [PATCH 076/232] Default LinMot speed resolution to standard as per manual --- motorApp/LinMotSrc/drvLinMot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/LinMotSrc/drvLinMot.h b/motorApp/LinMotSrc/drvLinMot.h index 0ce93b616..af910c76a 100644 --- a/motorApp/LinMotSrc/drvLinMot.h +++ b/motorApp/LinMotSrc/drvLinMot.h @@ -35,7 +35,7 @@ struct LinMotController asynUser *pasynUser; /* asyn */ int n_axes; /* Number of axes on this controller */ char port[80]; /* asyn port name */ - int speed_resolution; /* The motor speed resolution */ + int speed_resolution = 190735; /* The motor speed resolution. Default set to LinMot standard */ }; RTN_STATUS LinMotSetup(int, int); From a90d36a8ce1b64ce8ff084d3b9198986184b0959 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 11 Apr 2017 09:39:43 +0100 Subject: [PATCH 077/232] Can't initialise in class --- motorApp/LinMotSrc/drvLinMot.cc | 2 ++ motorApp/LinMotSrc/drvLinMot.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index cc3afab51..0ff55c287 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -370,6 +370,8 @@ RTN_STATUS LinMotConfig(int card, /* Card being configured */ motor_state[card]->DevicePrivate = malloc(sizeof(struct LinMotController)); cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; cntrl->n_axes = n_axes; + /* Will be updated later but this is the default for a LinMot */ + cntrl->speed_resolution = = 190735; strcpy(cntrl->port, port); return(OK); } diff --git a/motorApp/LinMotSrc/drvLinMot.h b/motorApp/LinMotSrc/drvLinMot.h index af910c76a..85ffab08a 100644 --- a/motorApp/LinMotSrc/drvLinMot.h +++ b/motorApp/LinMotSrc/drvLinMot.h @@ -35,7 +35,7 @@ struct LinMotController asynUser *pasynUser; /* asyn */ int n_axes; /* Number of axes on this controller */ char port[80]; /* asyn port name */ - int speed_resolution = 190735; /* The motor speed resolution. Default set to LinMot standard */ + int speed_resolution; /* The motor speed resolution. Default set to LinMot standard */ }; RTN_STATUS LinMotSetup(int, int); From 7649515ac1bcc55db68b7309a444518fe9cf3778 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 11 Apr 2017 09:40:22 +0100 Subject: [PATCH 078/232] Typo --- motorApp/LinMotSrc/drvLinMot.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 0ff55c287..0f2904a61 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -371,7 +371,7 @@ RTN_STATUS LinMotConfig(int card, /* Card being configured */ cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; cntrl->n_axes = n_axes; /* Will be updated later but this is the default for a LinMot */ - cntrl->speed_resolution = = 190735; + cntrl->speed_resolution = 190735; strcpy(cntrl->port, port); return(OK); } From 2ae633ac82eb8b9b7959477bdebda4cc23c87729 Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Mon, 10 Apr 2017 15:19:07 +0100 Subject: [PATCH 079/232] Update linmot_extra.db --- motorApp/Db/linmot_extra.db | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/Db/linmot_extra.db b/motorApp/Db/linmot_extra.db index a3ff94d13..a8331a79e 100644 --- a/motorApp/Db/linmot_extra.db +++ b/motorApp/Db/linmot_extra.db @@ -1,3 +1,3 @@ record(motor, "$(P)$(M)") { field(OFF, "$(OFF)") -} \ No newline at end of file +} From 2bfba3c37daffe430a05b734a7536ea9afd0cf0c Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 11 Apr 2017 15:36:52 +0100 Subject: [PATCH 080/232] Remove debug output --- motorApp/LinMotSrc/devLinMot.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc index 9bf72ff3b..550fc3cf1 100644 --- a/motorApp/LinMotSrc/devLinMot.cc +++ b/motorApp/LinMotSrc/devLinMot.cc @@ -16,7 +16,7 @@ volatile int devLinMotDebug = 0; extern "C" {epicsExportAddress(int, devLinMotDebug);} static inline void Debug(int level, const char *format, ...) { - if (true) { + if (level < devLinMotDebug) { va_list pVar; va_start(pVar, format); vprintf(format, pVar); @@ -189,7 +189,6 @@ STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct m break; case SET_VELOCITY: /* Speed resolution in nm/s. Convert to mm/s. */ - Debug(2, "Speed %ld, %ld", ival, cntrl->speed_resolution); sprintf(buff, "!SV%ld%c;", int(ival*1000000.0/cntrl->speed_resolution), axis); break; case SET_ACCEL: From 2e2bc78940e30537ca97410fda45d3ec90f21751 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Mon, 15 May 2017 16:34:28 +0100 Subject: [PATCH 081/232] Don't wait for homing cancellation if mode not 1. Add some debug output --- motorApp/MclennanSrc/homing.st | 69 ++++++++++++++++++++++++++-------- 1 file changed, 53 insertions(+), 16 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index f07162c4f..7a68ca686 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -1,4 +1,4 @@ -program homing("MOTPV=xxx,MODE,AXIS") +program homing("MOTPV=xxx,MODE,AXIS,DEBUG") #include "seqPVmacros.h" @@ -13,6 +13,7 @@ assign position to "{MOTPV}.VAL"; assign position_d to "{MOTPV}.DVAL"; int debug_flag; +int debug; int mode; int axis; @@ -33,6 +34,7 @@ ss motor /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0 */ mode = atoi(macValueGet("MODE")); axis = atoi(macValueGet("AXIS")); + axis = atoi(macValueGet("DEBUG")); printf("Sequencer: Homing mode for axis %i set to %i\n", axis, mode); } state ready } @@ -41,36 +43,63 @@ ss motor { when (home_reverse_pv==1) { + if (debug) { + printf("Sequencer: FROM ready TO reverse_home_requested\n"); + } } state reverse_home_requested when (home_forward_pv==1) { + if (debug) { + printf("Sequencer: FROM ready TO forward_home_requested\n"); + } } state forward_home_requested } state forward_home_requested { - when (home_forward_pv==0) { - if ( mode==1 ) { - jog_forward_value = 1; - pvPut(jog_forward_value); + /* In mode 1 we need to wait for the home to be cancelled before requesting a jog */ + when (home_forward_pv==0 && mode==1) { + jog_forward_value = 1; + pvPut(jog_forward_value); + if (debug) { + printf("Sequencer: FROM forward_home_requested TO processing_move_request (mode 1)\n"); } - } state processing_jog_request + } state processing_move_request + + /* No delay needed for modes other than 1 */ + when (mode!=1) { + if (debug) { + printf("Sequencer: FROM forward_home_requested TO processing_move_request\n"); + } + } state processing_move_request } state reverse_home_requested { - when (home_reverse_pv==0) { - if ( mode==1 ) { - jog_reverse_value = 1; - pvPut(jog_reverse_value); + /* In mode 1 we need to wait for the home to be cancelled before requesting a jog */ + when (mode==1 && home_reverse_pv==0) { + jog_reverse_value = 1; + pvPut(jog_reverse_value); + if (debug) { + printf("Sequencer: FROM reverse_home_requested TO processing_move_request (mode 1)\n"); + } + } state processing_move_request + + /* No delay needed for modes other than 1 */ + when (mode!=1) { + if (debug) { + printf("Sequencer: FROM reverse_home_requested TO processing_move_request\n"); } - } state processing_jog_request + } state processing_move_request } - state processing_jog_request + state processing_move_request { when (movable==0) { + if (debug) { + printf("Sequencer: FROM processing_move_request TO moving\n"); + } } state moving } @@ -90,15 +119,23 @@ ss motor set = 0; pvPut(set); } + if (debug) { + printf("Sequencer: FROM moving TO done\n"); + } } state done } state done { when () { - jog_reverse_value = 0; - pvPut(jog_reverse_value); - jog_forward_value = 0; - pvPut(jog_forward_value); + if ( mode==1 ) { + jog_reverse_value = 0; + pvPut(jog_reverse_value); + jog_forward_value = 0; + pvPut(jog_forward_value); + } + if (debug) { + printf("Sequencer: FROM done TO ready\n"); + } } state ready } } From 47b6460f55b0206702af888779f632ba1ff45039 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Mon, 22 May 2017 10:22:27 +0100 Subject: [PATCH 082/232] Assign to correct variable --- motorApp/MclennanSrc/homing.st | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 7a68ca686..70069432a 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -34,8 +34,11 @@ ss motor /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0 */ mode = atoi(macValueGet("MODE")); axis = atoi(macValueGet("AXIS")); - axis = atoi(macValueGet("DEBUG")); + debug = atoi(macValueGet("DEBUG")); printf("Sequencer: Homing mode for axis %i set to %i\n", axis, mode); + if (debug) { + printf("Sequencer: Debug mode ON"); + } } state ready } From 53ba9a0225e2fe5b938dedb44d2b34c74bef70d6 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Mon, 8 May 2017 10:50:12 +0100 Subject: [PATCH 083/232] Add McLen and LinMot motors to motion tracking --- motorApp/Db/motorUtil.db | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 1db70878b..dbaf9a37c 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -60,6 +60,11 @@ $(IFDMC07=#) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G CA") $(IFDMC08=#) field(OUTH, "$(PVPREFIX)CS:MOT:MOVING.H CA") $(IFIOC_SMC100_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.A CA") $(IFIOC_CONEXAGP_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.B CA") +$(IFIOC_MCLEN_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.C CA") +$(IFIOC_MCLEN_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.D CA") +$(IFIOC_MCLEN_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.E CA") +$(IFIOC_LINMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.F CA") +$(IFIOC_LINMOT_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.G CA") } # force periodoc processing to catch up any missing, shouldn't From 807bc93404e43f5ba5be04e76ec6503652e35793 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Wed, 10 May 2017 11:38:14 +0100 Subject: [PATCH 084/232] Expose DMOV as PV so it can be used in genie_python waitfor_move --- motorApp/Db/motorStatus.db | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/motorApp/Db/motorStatus.db b/motorApp/Db/motorStatus.db index 6ef2e4aea..a060c7650 100644 --- a/motorApp/Db/motorStatus.db +++ b/motorApp/Db/motorStatus.db @@ -29,4 +29,13 @@ record(aSub, "$(P)$(M):_STATUSCALC") record(stringin, "$(P)$(M)_STATUS") { field(DESC, "Simple Text version of motor MSTA") +} + +# Expose the DMOV field as a PV record, so it can be access from a block +record(ai,"$(P)$(M):DMOV") +{ + field(DESC, "Done move?") + field(SCAN, "Passive") + field(DTYP, "Soft Channel") + field(INP, "$(P)$(M).DMOV CP") } \ No newline at end of file From 876503f62d8b56b821c613ea032d7efb5abe7760 Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Wed, 10 May 2017 11:41:17 +0100 Subject: [PATCH 085/232] Slight cosmetic changes to DMOV record --- motorApp/Db/motorStatus.db | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/motorApp/Db/motorStatus.db b/motorApp/Db/motorStatus.db index a060c7650..3ec4f86ad 100644 --- a/motorApp/Db/motorStatus.db +++ b/motorApp/Db/motorStatus.db @@ -31,11 +31,11 @@ record(stringin, "$(P)$(M)_STATUS") field(DESC, "Simple Text version of motor MSTA") } -# Expose the DMOV field as a PV record, so it can be access from a block -record(ai,"$(P)$(M):DMOV") +# Expose the DMOV field as a PV record, so it can be accessed from a block +record(ai, "$(P)$(M):DMOV") { - field(DESC, "Done move?") + field(DESC, "Done moving?") field(SCAN, "Passive") field(DTYP, "Soft Channel") field(INP, "$(P)$(M).DMOV CP") -} \ No newline at end of file +} From 8e622c62b08507a043ef3128233a4f8de350d50c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 9 Jun 2017 18:33:25 +0100 Subject: [PATCH 086/232] Remove dbd includes for 3.15 --- motorApp/PIGCS2Src/PI_GCS2Support.dbd | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motorApp/PIGCS2Src/PI_GCS2Support.dbd b/motorApp/PIGCS2Src/PI_GCS2Support.dbd index 0252e014a..08768acb5 100644 --- a/motorApp/PIGCS2Src/PI_GCS2Support.dbd +++ b/motorApp/PIGCS2Src/PI_GCS2Support.dbd @@ -1,5 +1,5 @@ -include "asyn.dbd" -include "motorRecord.dbd" +#include "asyn.dbd" +#include "motorRecord.dbd" ##device(motor,VME_IO,devPIasyn,"PI Motor GCS2") ##driver(PIasyn) registrar(PIasynDriverRegister) From cedd842c65ccf3b26dabf80650db2cec283a8cb4 Mon Sep 17 00:00:00 2001 From: AdrianPotter Date: Tue, 13 Jun 2017 15:30:42 +0100 Subject: [PATCH 087/232] Apply patch from https://github.com/epics-modules/motor/pull/36 --- motorApp/MotorSrc/motorRecord.cc | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/motorApp/MotorSrc/motorRecord.cc b/motorApp/MotorSrc/motorRecord.cc index 12dae9b4d..4a9b73eeb 100644 --- a/motorApp/MotorSrc/motorRecord.cc +++ b/motorApp/MotorSrc/motorRecord.cc @@ -967,7 +967,12 @@ that it will happen when we return. ******************************************************************************/ static void maybeRetry(motorRecord * pmr) { - if ((fabs(pmr->diff) >= pmr->rdbd) && !pmr->hls && !pmr->lls) + bool user_cdir; + + /* Commanded direction in user coordinates. */ + user_cdir = ((pmr->dir == motorDIR_Pos) == (pmr->mres >= 0)) ? pmr->cdir : !pmr->cdir; + + if ((fabs(pmr->diff) >= pmr->rdbd) && !(pmr->hls && user_cdir) && !(pmr->lls && !user_cdir)) { /* No, we're not close enough. Try again. */ Debug(1, "maybeRetry: not close enough; diff = %f\n", pmr->diff); @@ -1274,7 +1279,7 @@ static long process(dbCommon *arg) } /* Do another update after LS error. */ - if (pmr->mip != MIP_DONE && (pmr->rhls || pmr->rlls)) + if (pmr->mip != MIP_DONE && ((pmr->rhls && pmr->cdir) || (pmr->rlls && !pmr->cdir))) { /* Restore DMOV to false and UNMARK it so it is not posted. */ pmr->dmov = FALSE; @@ -1304,7 +1309,7 @@ static long process(dbCommon *arg) } /* Are we "close enough" to desired position? */ - if (pmr->dmov == TRUE && !(pmr->rhls || pmr->rlls)) + if (pmr->dmov == TRUE && !((pmr->rhls && pmr->cdir) || (pmr->rlls && !pmr->cdir))) { mmap_bits.All = pmr->mmap; /* Initialize for MARKED. */ @@ -3244,7 +3249,8 @@ static void alarm_sub(motorRecord * pmr) status = recGblSetSevr((dbCommon *) pmr, UDF_ALARM, INVALID_ALARM); return; } - /* limit-switch and soft-limit violations */ + /* Limit-switch and soft-limit violations. Consider limit switches also if not in + * direction of move (limit hit by externally triggered move) */ if (pmr->hlsv && (pmr->hls || (pmr->dval > pmr->dhlm))) { status = recGblSetSevr((dbCommon *) pmr, HIGH_ALARM, pmr->hlsv); @@ -3561,11 +3567,12 @@ static void if (pmr->tdir != old_tdir) MARK(M_TDIR); - /* Get states of high, low limit switches. */ - pmr->rhls = (msta.Bits.RA_PLUS_LS) && pmr->cdir; - pmr->rlls = (msta.Bits.RA_MINUS_LS) && !pmr->cdir; + /* Get states of high, low limit switches. State is independent of direction. */ + pmr->rhls = (msta.Bits.RA_PLUS_LS); + pmr->rlls = (msta.Bits.RA_MINUS_LS); - ls_active = (pmr->rhls || pmr->rlls) ? true : false; + /* Treat limit switch active only when it is pressed and in direction of movement. */ + ls_active = ((pmr->rhls && pmr->cdir) || (pmr->rlls && !pmr->cdir)) ? true : false; pmr->hls = ((pmr->dir == motorDIR_Pos) == (pmr->mres >= 0)) ? pmr->rhls : pmr->rlls; pmr->lls = ((pmr->dir == motorDIR_Pos) == (pmr->mres >= 0)) ? pmr->rlls : pmr->rhls; From f627f92e22bfda78ed00b048703ecb5c7b8ef5cb Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Tue, 15 Aug 2017 15:09:10 +0100 Subject: [PATCH 088/232] Added third linmot to fan --- motorApp/Db/motorUtil.db | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index dbaf9a37c..2ed942190 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -65,6 +65,7 @@ $(IFIOC_MCLEN_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.D CA") $(IFIOC_MCLEN_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.E CA") $(IFIOC_LINMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.F CA") $(IFIOC_LINMOT_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.G CA") +$(IFIOC_LINMOT_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.H CA") } # force periodoc processing to catch up any missing, shouldn't From 78827605967e211674f55a5626c6f3d4d6f05fda Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 13 Jul 2017 22:34:29 +0100 Subject: [PATCH 089/232] Add BKHOFF and PIMOT --- motorApp/Db/motorUtil.db | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 2ed942190..31bf1abec 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -46,6 +46,15 @@ record(calcout, "$(P)_MOVING") field(OUT, "$(P)_FAN PP") } +## this file get loaded by each motor IOC, it needs to +## set a different calc field in MOVING for each IOC +## +## it is OK to reuse OUTA in the lines when they refer to a +## specific IOC and no two lines will get loaded at one. However +## if is IMPORTANT to not re-use one of the outputs on the +## MOVING or _MOVING calc record target +## galils done in a slightly diffrent way for historical reasons, +## will be fixed in upcoming ticket. record(dfanout, "$(P)_FAN") { field(OMSL, "supervisory") @@ -66,9 +75,11 @@ $(IFIOC_MCLEN_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.E CA") $(IFIOC_LINMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.F CA") $(IFIOC_LINMOT_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.G CA") $(IFIOC_LINMOT_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.H CA") +$(IFIOC_BKHOFF_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.I CA") +$(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") } -# force periodoc processing to catch up any missing, shouldn't +# force periodic processing to catch up any missing, shouldn't # be necessaryu but no harm to be careful record(fanout, "$(P)_FAN2") { From a266f288725e3967dbea82e536f410711e18539d Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Thu, 16 Nov 2017 17:43:23 +0000 Subject: [PATCH 090/232] Added archiving to base motor record --- motorApp/Db/basic_motor.db | 3 +++ 1 file changed, 3 insertions(+) diff --git a/motorApp/Db/basic_motor.db b/motorApp/Db/basic_motor.db index 4aea3c4fa..ef2698749 100644 --- a/motorApp/Db/basic_motor.db +++ b/motorApp/Db/basic_motor.db @@ -17,6 +17,9 @@ grecord(motor,"$(P)$(M)") field(DLLM,"$(DLLM)") field(INIT,"$(INIT)") field(TWV,"1") + # ISIS local archiving and alarm + info(archive, "0.02 VAL RBV DVAL OFF MSTA DIR CNEN MOVN DMOV MISS RCNT") + info(alarm, "Motors") } # ISIS local aliases for genie_python From cc2855d4aac603fe001bea17576d955c0f06c4dd Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Fri, 17 Nov 2017 12:31:33 +0000 Subject: [PATCH 091/232] Added additional fields to archive --- motorApp/Db/asyn_motor.db | 3 +++ motorApp/Db/basic_asyn_motor.db | 3 +++ motorApp/Db/motor.db | 3 +++ 3 files changed, 9 insertions(+) diff --git a/motorApp/Db/asyn_motor.db b/motorApp/Db/asyn_motor.db index 7df37a55f..4776cbd5f 100644 --- a/motorApp/Db/asyn_motor.db +++ b/motorApp/Db/asyn_motor.db @@ -23,6 +23,9 @@ record(motor, "$(P)$(M)") { field(RTRY, "$(RTRY=10)") field(TWV, "1") field(SDIS, "$(P)$(M)_able.VAL") + # ISIS local archiving and alarm + info(archive, "0.02 VAL RBV DVAL OFF MSTA DIR CNEN MOVN DMOV MISS RCNT") + info(alarm, "Motors") } # ISIS local aliases for genie_python diff --git a/motorApp/Db/basic_asyn_motor.db b/motorApp/Db/basic_asyn_motor.db index 6ae5db91d..1e40d6f7f 100644 --- a/motorApp/Db/basic_asyn_motor.db +++ b/motorApp/Db/basic_asyn_motor.db @@ -17,6 +17,9 @@ record(motor,"$(P)$(M)") field(DLLM,"$(DLLM)") field(INIT,"$(INIT)") field(TWV,"1") + # ISIS local archiving and alarm + info(archive, "0.02 VAL RBV DVAL OFF MSTA DIR CNEN MOVN DMOV MISS RCNT") + info(alarm, "Motors") } # ISIS local aliases for genie_python diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 429cbc9dd..b2bf4cf12 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -24,6 +24,9 @@ record(motor, "$(P)$(M)") { field(INIT, "$(INIT)") field(TWV, "1") field(SDIS, "$(P)$(M)_able.VAL") + # ISIS local archiving and alarm + info(archive, "0.02 VAL RBV DVAL OFF MSTA DIR CNEN MOVN DMOV MISS RCNT") + info(alarm, "Motors") } # ISIS local aliases for genie_python From 71328831d9869ab6745881ea4eada19f77678885 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Fri, 17 Nov 2017 16:40:25 +0000 Subject: [PATCH 092/232] Changed galil to be more like other IOCs --- motorApp/Db/motorUtil.db | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 31bf1abec..db3e13d42 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -53,20 +53,20 @@ record(calcout, "$(P)_MOVING") ## specific IOC and no two lines will get loaded at one. However ## if is IMPORTANT to not re-use one of the outputs on the ## MOVING or _MOVING calc record target -## galils done in a slightly diffrent way for historical reasons, -## will be fixed in upcoming ticket. record(dfanout, "$(P)_FAN") { field(OMSL, "supervisory") field(SELM, "All") -$(IFDMC01=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.A CA") -$(IFDMC02=#) field(OUTB, "$(PVPREFIX)CS:MOT:MOVING.B CA") -$(IFDMC03=#) field(OUTC, "$(PVPREFIX)CS:MOT:MOVING.C CA") -$(IFDMC04=#) field(OUTD, "$(PVPREFIX)CS:MOT:MOVING.D CA") -$(IFDMC05=#) field(OUTE, "$(PVPREFIX)CS:MOT:MOVING.E CA") -$(IFDMC06=#) field(OUTF, "$(PVPREFIX)CS:MOT:MOVING.F CA") -$(IFDMC07=#) field(OUTG, "$(PVPREFIX)CS:MOT:MOVING.G CA") -$(IFDMC08=#) field(OUTH, "$(PVPREFIX)CS:MOT:MOVING.H CA") +$(IFIOC_GALIL_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.A CA") +$(IFIOC_GALIL_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.B CA") +$(IFIOC_GALIL_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.C CA") +$(IFIOC_GALIL_04=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.D CA") +$(IFIOC_GALIL_05=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.E CA") +$(IFIOC_GALIL_06=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.F CA") +$(IFIOC_GALIL_07=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.G CA") +$(IFIOC_GALIL_08=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.H CA") +$(IFIOC_GALIL_09=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.I CA") +$(IFIOC_GALIL_10=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.J CA") $(IFIOC_SMC100_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.A CA") $(IFIOC_CONEXAGP_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.B CA") $(IFIOC_MCLEN_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.C CA") @@ -80,7 +80,7 @@ $(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") } # force periodic processing to catch up any missing, shouldn't -# be necessaryu but no harm to be careful +# be necessary but no harm to be careful record(fanout, "$(P)_FAN2") { field(SCAN, "10 second") From 4f6aa6ae161e1066cb2e1e3f4c727926123d9992 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Fri, 17 Nov 2017 17:08:37 +0000 Subject: [PATCH 093/232] Fixed typo --- motorApp/Db/motorUtil.db | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index db3e13d42..1115804ed 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -51,7 +51,7 @@ record(calcout, "$(P)_MOVING") ## ## it is OK to reuse OUTA in the lines when they refer to a ## specific IOC and no two lines will get loaded at one. However -## if is IMPORTANT to not re-use one of the outputs on the +## it is IMPORTANT to not re-use one of the outputs on the ## MOVING or _MOVING calc record target record(dfanout, "$(P)_FAN") { From 4522a7e057c83b69f35a36c38559ac9e2c8653fd Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Thu, 21 Dec 2017 09:25:01 +0000 Subject: [PATCH 094/232] Send the reset command after every move. Configurable at startup --- motorApp/MclennanSrc/MclennanRegister.cc | 9 +++++---- motorApp/MclennanSrc/devPM304.cc | 5 ++++- motorApp/MclennanSrc/drvPM304.cc | 10 ++++++---- motorApp/MclennanSrc/drvPM304.h | 5 +++-- 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/motorApp/MclennanSrc/MclennanRegister.cc b/motorApp/MclennanSrc/MclennanRegister.cc index 0dda3ff9c..56a290e22 100644 --- a/motorApp/MclennanSrc/MclennanRegister.cc +++ b/motorApp/MclennanSrc/MclennanRegister.cc @@ -34,13 +34,14 @@ static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; static const iocshArg configArg1 = {"asyn port name", iocshArgString}; static const iocshArg configArg2 = {"Number of axes", iocshArgInt}; static const iocshArg configArg3 = {"Combined home modes for all axes", iocshArgInt}; +static const iocshArg configArg4 = {"Reset before move", iocshArgInt}; static const iocshArg * const PM304SetupArgs[2] = {&setupArg0, &setupArg1}; -static const iocshArg * const PM304ConfigArgs[4] = {&configArg0, &configArg1, - &configArg2, &configArg3}; +static const iocshArg * const PM304ConfigArgs[5] = {&configArg0, &configArg1, + &configArg2, &configArg3, &configArg4}; static const iocshFuncDef setupPM304 = {"PM304Setup", 2, PM304SetupArgs}; -static const iocshFuncDef configPM304 = {"PM304Config", 4, PM304ConfigArgs}; +static const iocshFuncDef configPM304 = {"PM304Config", 5, PM304ConfigArgs}; static void setupPM304CallFunc(const iocshArgBuf *args) { @@ -48,7 +49,7 @@ static void setupPM304CallFunc(const iocshArgBuf *args) } static void configPM304CallFunc(const iocshArgBuf *args) { - PM304Config(args[0].ival, args[1].sval, args[2].ival, args[3].ival); + PM304Config(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival); } static void MclennanRegister(void) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 78d80afbf..a025d9ab7 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -232,7 +232,10 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo if (strlen(mr->post) != 0) motor_call->postmsgptr = (char *) &mr->post; break; - + /* Send a reset command before any move */ + if (cntrl->reset_before_move==1) { + sprintf(buff, "%dRS;", axis); + } default: break; } diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 6e0a49058..0ce0a1dcb 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -565,10 +565,11 @@ PM304Setup(int num_cards, /* maximum number of controllers in system */ /* PM304Config() */ /*****************************************************/ RTN_STATUS -PM304Config(int card, /* card being configured */ - const char *port, /* asyn port name */ - int n_axes, /* Number of axes */ - int home_modes) /* Combined home modes of all axes */ +PM304Config(int card, /* card being configured */ + const char *port, /* asyn port name */ + int n_axes, /* Number of axes */ + int home_modes, /* Combined home modes of all axes */ + int reset_before_move) /* Reset the McLennan before every move */ { struct PM304controller *cntrl; @@ -586,6 +587,7 @@ PM304Config(int card, /* card being configured */ cntrl->home_mode[i] = int(home_modes/float(pow(double(n_modes), i)))%n_modes; printf("Homing axis %i using mode %i\n", i+1, cntrl->home_mode[i]); } + cntrl->reset_before_move = reset_before_move; cntrl->model = MODEL_PM304; /* Assume PM304 initially */ strcpy(cntrl->port, port); return(OK); diff --git a/motorApp/MclennanSrc/drvPM304.h b/motorApp/MclennanSrc/drvPM304.h index ace4bdf06..58561f667 100644 --- a/motorApp/MclennanSrc/drvPM304.h +++ b/motorApp/MclennanSrc/drvPM304.h @@ -37,10 +37,11 @@ struct PM304controller int model; /* Model = MODEL_PM304 or MODEL_PM600 */ int use_encoder[PM304_MAX_CHANNELS]; /* Does axis have an encoder? */ int home_mode[PM304_MAX_CHANNELS]; /* The combined home modes for all axes */ - char port[80]; /* asyn port name */ + char port[80]; /* Asyn port name */ + int reset_before_move; /* Reset the controller before any move command */ }; RTN_STATUS PM304Setup(int, int); -RTN_STATUS PM304Config(int, const char *, int, int); +RTN_STATUS PM304Config(int, const char *, int, int, int); #endif /* INCdrvPM304h */ From 41df87187c363f7c43c46701db7eb0994d50c36f Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Wed, 3 Jan 2018 09:58:06 +0000 Subject: [PATCH 095/232] Add string to buffer before send --- motorApp/MclennanSrc/devPM304.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index a025d9ab7..08e8e3dcc 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -231,11 +231,12 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo } if (strlen(mr->post) != 0) motor_call->postmsgptr = (char *) &mr->post; - break; - /* Send a reset command before any move */ if (cntrl->reset_before_move==1) { sprintf(buff, "%dRS;", axis); + strcat(motor_call->message, buff); } + break; + /* Send a reset command before any move */ default: break; } From c460eea5914a3c88cd395c7037790919181b8f33 Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Wed, 3 Jan 2018 10:14:11 +0000 Subject: [PATCH 096/232] Move comment to better place --- motorApp/MclennanSrc/devPM304.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 08e8e3dcc..125fa1431 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -231,12 +231,12 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo } if (strlen(mr->post) != 0) motor_call->postmsgptr = (char *) &mr->post; + /* Send a reset command before any move */ if (cntrl->reset_before_move==1) { sprintf(buff, "%dRS;", axis); strcat(motor_call->message, buff); } break; - /* Send a reset command before any move */ default: break; } From d8e273dcbfba53dbdbd516eae6faf7f5bbac6496 Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Tue, 9 Jan 2018 17:39:41 +0000 Subject: [PATCH 097/232] Give active resets more urgent debug level than normal replies --- motorApp/MclennanSrc/drvPM304.cc | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 0ce0a1dcb..e5cc87487 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -375,13 +375,17 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) * characters. The PM304 cannot handle more than 1 command on a line * so send them separately */ strcpy(temp, com); - for (p = epicsStrtok_r(temp, ";", &tok_save); + for (p = epicsStrtok_r(temp, ";", &tok_save); ((p != NULL) && (strlen(p) != 0)); p = epicsStrtok_r(NULL, ";", &tok_save)) { + Debug(2, "send_mess: sending message to card %d, message=%s\n", card, p); - pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, + pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); - Debug(2, "send_mess: card %d, response=%s\n", card, response); + /* Set the debug level for most responses to be 2. Flag reset messages + * to 1 so we can spot them more easily */ + int level = strcmp(&p[1],"RS")==0 && strstr(response, "NOT ABORTED") == NULL ? 1 : 2; + Debug(level, "send_mess: card %d, response=...\n%s\n", card, response); } return(OK); From 8df29b37801a62d6501f01efae6ef7f954967e35 Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Wed, 10 Jan 2018 09:18:05 +0000 Subject: [PATCH 098/232] Add problem flag for reset error so that flagged in GUI as alarm --- motorApp/MclennanSrc/drvPM304.cc | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index e5cc87487..842cb4c27 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -83,6 +83,7 @@ static inline void Debug(int level, const char *format, ...) { */ int PM304_num_cards = 0; +int controller_error = 0; /* Local data required for every driver; see "motordrvComCode.h" */ #include "motordrvComCode.h" @@ -314,6 +315,9 @@ STATIC int set_status(int card, int signal) motor_info->encoder_position = motorData; motor_info->no_motion_count = 0; } + + /* Problem if controller error */ + status.Bits.RA_PROBLEM = controller_error > 0 ? 1 : status.Bits.RA_PROBLEM; /* Parse motor velocity? */ /* NEEDS WORK */ @@ -384,7 +388,14 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); /* Set the debug level for most responses to be 2. Flag reset messages * to 1 so we can spot them more easily */ - int level = strcmp(&p[1],"RS")==0 && strstr(response, "NOT ABORTED") == NULL ? 1 : 2; + int level; + if (strcmp(&p[1],"RS")==0 && strstr(response, "NOT ABORTED") == NULL) { + level = 1; + controller_error = 1; + } else { + level = 2; + controller_error = 0; + } Debug(level, "send_mess: card %d, response=...\n%s\n", card, response); } From 6f6943a8d03234f21cbfddf59f783bd7e174fcc2 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 11 Jan 2018 11:04:44 +0000 Subject: [PATCH 099/232] Fix dependency --- motorApp/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/Makefile b/motorApp/Makefile index ea6d381fe..807fd6eaa 100644 --- a/motorApp/Makefile +++ b/motorApp/Makefile @@ -40,7 +40,7 @@ DIRS += MclennanSrc MclennanSrc_DEPEND_DIRS = MotorSrc DIRS += LinMotSrc -MclennanSrc_DEPEND_DIRS = MotorSrc +LinMotSrc_DEPEND_DIRS = MotorSrc DIRS += PiSrc PiSrc_DEPEND_DIRS = MotorSrc From e59bb748bfad5fac9e00c82e98afb157eb6d9536 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 19 Jan 2018 16:37:04 +0000 Subject: [PATCH 100/232] Update casts for latest gcc --- motorApp/AcsSrc/drvMCB4B.cc | 2 +- motorApp/AcsTech80Src/drvSPiiPlus.cc | 2 +- motorApp/AerotechSrc/drvSoloist.cc | 30 +++++++++++------------ motorApp/ImsSrc/drvIM483SM.cc | 16 ++++++------- motorApp/KohzuSrc/drvSC800.cc | 8 +++---- motorApp/LinMotSrc/drvLinMot.cc | 2 +- motorApp/MclennanSrc/drvPM304.cc | 2 +- motorApp/MicosSrc/drvMicos.cc | 2 +- motorApp/MicroMoSrc/drvMVP2001.cc | 20 ++++++++-------- motorApp/MotorSrc/motordevCom.cc | 4 ++-- motorApp/NewportSrc/drvESP300.cc | 18 +++++++------- motorApp/NewportSrc/drvMM3000.cc | 16 ++++++------- motorApp/NewportSrc/drvMM4000.cc | 36 ++++++++++++++-------------- motorApp/NewportSrc/drvPM500.cc | 18 +++++++------- motorApp/OmsSrc/devOmsCom.cc | 2 +- motorApp/OmsSrc/drvMAXv.cc | 16 ++++++------- motorApp/OmsSrc/drvOms.cc | 10 ++++---- motorApp/OmsSrc/drvOms58.cc | 10 ++++---- motorApp/OmsSrc/drvOmsPC68.cc | 12 +++++----- motorApp/OrielSrc/drvEMC18011.cc | 2 +- motorApp/PC6KSrc/drvPC6K.cc | 10 ++++---- motorApp/PiJenaSrc/drvPIJEDS.cc | 4 ++-- motorApp/PiSrc/drvPIC662.cc | 10 ++++---- motorApp/PiSrc/drvPIC663.cc | 8 +++---- motorApp/PiSrc/drvPIC844.cc | 10 ++++---- motorApp/PiSrc/drvPIC848.cc | 4 ++-- motorApp/PiSrc/drvPIC862.cc | 8 +++---- motorApp/PiSrc/drvPIE516.cc | 16 ++++++------- motorApp/PiSrc/drvPIE710.cc | 4 ++-- motorApp/PiSrc/drvPIE816.cc | 4 ++-- motorApp/ThorLabsSrc/drvMDT695.cc | 2 +- 31 files changed, 154 insertions(+), 154 deletions(-) diff --git a/motorApp/AcsSrc/drvMCB4B.cc b/motorApp/AcsSrc/drvMCB4B.cc index ea182924f..a93be66d9 100644 --- a/motorApp/AcsSrc/drvMCB4B.cc +++ b/motorApp/AcsSrc/drvMCB4B.cc @@ -262,7 +262,7 @@ STATIC int set_status(int card, int signal) /* Test for post-move string. */ if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 && nodeptr->postmsgptr != 0) { - send_mess(card, nodeptr->postmsgptr, (char) NULL); + send_mess(card, nodeptr->postmsgptr, (char*) NULL); /* The MCB4B always sends back a response, read it and discard */ recv_mess(card, buff, WAIT); nodeptr->postmsgptr = NULL; diff --git a/motorApp/AcsTech80Src/drvSPiiPlus.cc b/motorApp/AcsTech80Src/drvSPiiPlus.cc index 91f124eb0..7a56cb6a2 100644 --- a/motorApp/AcsTech80Src/drvSPiiPlus.cc +++ b/motorApp/AcsTech80Src/drvSPiiPlus.cc @@ -406,7 +406,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strncpy(send_buff, nodeptr->postmsgptr, 80); - send_mess(card, send_buff, (char) NULL); + send_mess(card, send_buff, (char*) NULL); nodeptr->postmsgptr = NULL; } diff --git a/motorApp/AerotechSrc/drvSoloist.cc b/motorApp/AerotechSrc/drvSoloist.cc index 6b95a1fc2..61d5bd02a 100644 --- a/motorApp/AerotechSrc/drvSoloist.cc +++ b/motorApp/AerotechSrc/drvSoloist.cc @@ -220,7 +220,7 @@ static int set_status(int card, int signal) // get the axis status sprintf(buff, "AXISSTATUS()"); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); comm_status = recv_mess(card, buff, 1); if (comm_status > 0 && buff[0] == ASCII_ACK_CHAR) { @@ -273,7 +273,7 @@ static int set_status(int card, int signal) /* get the axis fault status */ sprintf(buff, "AXISFAULT()"); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); comm_status = recv_mess(card, buff, 1); axis_status = atoi(&buff[1]); status.Bits.RA_PLUS_LS = axis_status & CW_FAULT_BIT ? 1 : 0; @@ -287,7 +287,7 @@ static int set_status(int card, int signal) // get the position sprintf(buff, "PFBKPROG()"); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); recv_mess(card, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { @@ -339,7 +339,7 @@ static int set_status(int card, int signal) if ((status.Bits.RA_DONE || ls_active) && nodeptr != 0 && nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -593,7 +593,7 @@ static int motor_init() // we only care if we get a response // so we don't need to send a valid command strcpy(buff, "NONE"); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); status = recv_mess(card_index, buff, 1); retry++; @@ -605,7 +605,7 @@ static int motor_init() brdptr->motor_in_motion = 0; // Read controller ID string strcpy(buff, "GETPARM(265)"); //UserString1 - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { @@ -622,7 +622,7 @@ static int motor_init() { // Does this axis actually exist? sprintf(buff, "GETPARM(257)"); //AxisName - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); // We know the axis exists if we got an ACK response @@ -647,7 +647,7 @@ static int motor_init() // Determine if encoder present based on open/closed loop mode. sprintf(buff, "GETPARM(58)"); //CfgFbkPosType - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { @@ -660,7 +660,7 @@ static int motor_init() // Determine if gains are supported based on the motor type. sprintf(buff, "GETPARM(33)"); //CfgMotType - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { @@ -673,12 +673,12 @@ static int motor_init() // Stop all motors sprintf(buff, "ABORT"); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); // Determine drive resolution sprintf(buff, "GETPARM(3)"); //PosScaleFactor - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { @@ -698,7 +698,7 @@ static int motor_init() // Save home preset position sprintf(buff, "GETPARM(108)"); //HomeOffset - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { @@ -707,7 +707,7 @@ static int motor_init() // Determine low limit sprintf(buff, "GETPARM(47)"); //ThresholdSoftCCW - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { @@ -716,7 +716,7 @@ static int motor_init() // Determine high limit sprintf(buff, "GETPARM(48)"); //ThresholdSoftCW - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { @@ -725,7 +725,7 @@ static int motor_init() // Save the HomeDirection parameter sprintf(buff, "GETPARM(106)"); //HomeDirection - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) { diff --git a/motorApp/ImsSrc/drvIM483SM.cc b/motorApp/ImsSrc/drvIM483SM.cc index 9db3f09cf..a727e568f 100644 --- a/motorApp/ImsSrc/drvIM483SM.cc +++ b/motorApp/ImsSrc/drvIM483SM.cc @@ -240,7 +240,7 @@ static int set_status(int card, int signal) nodeptr = motor_info->motor_motion; status.All = motor_info->status.All; - send_mess(card, "^", (char) NULL); + send_mess(card, "^", (char*) NULL); rtn_state = recv_mess(card, buff, 1); if (rtn_state > 0) { @@ -275,7 +275,7 @@ static int set_status(int card, int signal) * Skip to substring for this motor, convert to double */ - send_mess(card, "Z 0", (char) NULL); + send_mess(card, "Z 0", (char*) NULL); recv_mess(card, buff, 1); motorData = atof(&buff[5]); @@ -297,7 +297,7 @@ static int set_status(int card, int signal) plusdir = (status.Bits.RA_DIRECTION) ? true : false; - send_mess(card, "] 0", (char) NULL); + send_mess(card, "] 0", (char*) NULL); recv_mess(card, buff, 1); rtnval = atoi(&buff[5]); @@ -320,7 +320,7 @@ static int set_status(int card, int signal) else status.Bits.RA_MINUS_LS = 0; - send_mess(card, "] 1", (char) NULL); + send_mess(card, "] 1", (char*) NULL); recv_mess(card, buff, 1); rtnval = buff[5]; @@ -338,7 +338,7 @@ static int set_status(int card, int signal) motor_info->encoder_position = 0; else { - send_mess(card, "z 0", (char) NULL); + send_mess(card, "z 0", (char*) NULL); recv_mess(card, buff, 1); motorData = atof(&buff[5]); motor_info->encoder_position = (epicsInt32) motorData; @@ -361,7 +361,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -553,9 +553,9 @@ static int motor_init() /* flush any junk at input port - should not be any data available */ pasynOctetSyncIO->flush(cntrl->pasynUser); - send_mess(card_index, "\003", (char) NULL); /* Reset device. */ + send_mess(card_index, "\003", (char*) NULL); /* Reset device. */ epicsThreadSleep(1.0); - send_mess(card_index, " ", (char) NULL); + send_mess(card_index, " ", (char*) NULL); /* Save controller identification message. */ src = buff; diff --git a/motorApp/KohzuSrc/drvSC800.cc b/motorApp/KohzuSrc/drvSC800.cc index 2bd2b89ab..61beea38e 100644 --- a/motorApp/KohzuSrc/drvSC800.cc +++ b/motorApp/KohzuSrc/drvSC800.cc @@ -223,7 +223,7 @@ static int set_status(int card, int signal) charcnt = recv_mess(card, buff, FLUSH); sprintf(buff,"STR1/%d",(signal + 1)); - send_mess(card, buff, (char) NULL); /* Tell Status */ + send_mess(card, buff, (char*) NULL); /* Tell Status */ charcnt = recv_mess(card, buff, 1); convert_cnt = sscanf(buff, "C\tSTR%d\t1\t%d\t%d\t%d\t%d\t%d\t%d\t%d", &str_axis, &str_move, &str_norg, &str_orgg, @@ -258,7 +258,7 @@ static int set_status(int card, int signal) /* Parse motor position */ sprintf(buff,"RDP%d/0", (signal + 1)); - send_mess(card, buff, (char) NULL); /* Tell Position */ + send_mess(card, buff, (char*) NULL); /* Tell Position */ recv_mess(card, buff, 1); convert_cnt = sscanf(buff, "C\tRDP%d\t%d", &str_axis, &motorData); @@ -287,7 +287,7 @@ static int set_status(int card, int signal) /* Torque enabled? */ sprintf(buff,"RSY%d/21", (signal + 1)); - send_mess(card, buff, (char) NULL); /* Tell Position */ + send_mess(card, buff, (char*) NULL); /* Tell Position */ recv_mess(card, buff, 1); convert_cnt = sscanf(buff, "C\tRSY%d\t21\t%d", &str_axis, &str_move); status.Bits.EA_POSITION = (str_move == 0) ? 1 : 0; @@ -329,7 +329,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc index 0f2904a61..a289e7d74 100644 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ b/motorApp/LinMotSrc/drvLinMot.cc @@ -204,7 +204,7 @@ STATIC int set_status(int card, int signal) { strcpy(buff, nodeptr->postmsgptr); strcat(buff, "\r"); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 842cb4c27..2b55f8212 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -336,7 +336,7 @@ STATIC int set_status(int card, int signal) { strcpy(buff, nodeptr->postmsgptr); strcat(buff, "\r"); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } diff --git a/motorApp/MicosSrc/drvMicos.cc b/motorApp/MicosSrc/drvMicos.cc index 04c0c98bc..2ac11f036 100644 --- a/motorApp/MicosSrc/drvMicos.cc +++ b/motorApp/MicosSrc/drvMicos.cc @@ -284,7 +284,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); /* The Micos will not send back a response for a 'set' command, don't need next line */ /* recv_mess(card, buff, WAIT); */ nodeptr->postmsgptr = NULL; diff --git a/motorApp/MicroMoSrc/drvMVP2001.cc b/motorApp/MicroMoSrc/drvMVP2001.cc index 4cf2063bb..01b103013 100644 --- a/motorApp/MicroMoSrc/drvMVP2001.cc +++ b/motorApp/MicroMoSrc/drvMVP2001.cc @@ -307,7 +307,7 @@ static int set_status(int card, int signal) statusStr[0] = positionStr[0] = buff[0] = '\0'; sprintf(buff, "%d ST", (signal + 1)); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); rtn_state = recv_mess(card, buff, 1); if (rtn_state > 0) { @@ -343,7 +343,7 @@ static int set_status(int card, int signal) status.Bits.RA_DONE = !mstat.Bits.inMotion; sprintf(buff, "%d POS", (signal + 1)); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); recv_mess(card, buff, 1); /* @@ -434,7 +434,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -645,7 +645,7 @@ static int motor_init() do { sprintf(buff, "%d ST", (total_axis + 1)); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); status = recv_mess(card_index, buff, 1); retry++; } while (status <= 0 && retry < 3); @@ -667,11 +667,11 @@ static int motor_init() /* stop and initialize the controller */ sprintf(buff, "%d V 0", (motor_index + 1)); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); sprintf(buff, "%d HO", (motor_index + 1)); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); sprintf(buff, "%d EN", (motor_index + 1)); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); motor_info->status.All = 0; motor_info->no_motion_count = 0; @@ -690,7 +690,7 @@ static int motor_init() limitStr[0] = '\0'; /* Determine low limit */ sprintf(buff, "%d LL -", (motor_index + 1)); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); strncat(limitStr, &buff[5], 8); motor_info->low_limit = (epicsInt32) strtoul(limitStr, NULL, 16); @@ -698,7 +698,7 @@ static int motor_init() limitStr[0] = '\0'; /* Determine high limit */ sprintf(buff, "%d LL", (motor_index + 1)); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); strncat(limitStr, &buff[5], 8); motor_info->high_limit = (epicsInt32) strtoul(limitStr, NULL, 16); @@ -716,7 +716,7 @@ static int motor_init() epicsThreadSleep(0.2); sprintf(buff, "%d HO", (motor_index + 1)); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); set_status(card_index, motor_index); /* Read status of each * motor */ diff --git a/motorApp/MotorSrc/motordevCom.cc b/motorApp/MotorSrc/motordevCom.cc index 38a96991f..f55724ecc 100644 --- a/motorApp/MotorSrc/motordevCom.cc +++ b/motorApp/MotorSrc/motordevCom.cc @@ -464,8 +464,8 @@ epicsShareFunc long motor_start_trans_com(struct motorRecord *mr, struct board_s motor_call->type = UNDEFINED; motor_call->mrecord = (struct dbCommon *) mr; motor_call->message[0] = (char) NULL; - motor_call->postmsgptr = (char) NULL; - motor_call->termstring = (char) NULL; + motor_call->postmsgptr = (char*) NULL; + motor_call->termstring = (char*) NULL; return (0); } diff --git a/motorApp/NewportSrc/drvESP300.cc b/motorApp/NewportSrc/drvESP300.cc index a1369bada..4d7609a2c 100644 --- a/motorApp/NewportSrc/drvESP300.cc +++ b/motorApp/NewportSrc/drvESP300.cc @@ -227,7 +227,7 @@ static int set_status(int card, int signal) status.All = motor_info->status.All; sprintf(outbuff, "%.2dMD", signal + 1); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); charcnt = recv_mess(card, inbuff, 1); if (charcnt == 1 && (inbuff[0] == '0' || inbuff[0] == '1')) @@ -257,7 +257,7 @@ static int set_status(int card, int signal) /* Get motor position. */ sprintf(outbuff, READ_POSITION, signal + 1); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); charcnt = recv_mess(card, inbuff, 1); motorData = atof(inbuff) / cntrl->drive_resolution[signal]; @@ -281,7 +281,7 @@ static int set_status(int card, int signal) /* Get travel limit switch status. */ sprintf(outbuff, "%.2dPH", signal + 1); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); charcnt = recv_mess(card, inbuff, 1); cptr = strchr(inbuff, 'H'); if (cptr == NULL) @@ -320,7 +320,7 @@ static int set_status(int card, int signal) /* Get motor power on/off status. */ sprintf(outbuff, "%.2dMO?", signal + 1); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); charcnt = recv_mess(card, inbuff, 1); power = atoi(inbuff) ? true : false; @@ -333,7 +333,7 @@ static int set_status(int card, int signal) /* Get error code. */ sprintf(outbuff, "%.2dTE?", signal + 1); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); charcnt = recv_mess(card, inbuff, 1); errcode = atoi(inbuff); if (errcode != 0) @@ -360,7 +360,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(outbuff, nodeptr->postmsgptr); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -640,7 +640,7 @@ static int motor_init() do { - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, buff, 1); retry++; /* Return value is length of response string */ @@ -654,7 +654,7 @@ static int motor_init() brdptr->motor_in_motion = 0; strcpy(brdptr->ident, &buff[1]); /* Skip "\n" */ - send_mess(card_index, "ZU", (char) NULL); + send_mess(card_index, "ZU", (char*) NULL); recv_mess(card_index, buff, 1); total_axis = buff[0] >> 4; if (total_axis > 4) @@ -666,7 +666,7 @@ static int motor_init() for (motor_index = 0; motor_index < total_axis; motor_index++) { sprintf(buff, STOP_AXIS, motor_index + 1); /* Stop motor */ - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); /* Initialize. */ brdptr->motor_info[motor_index].motor_motion = NULL; } diff --git a/motorApp/NewportSrc/drvMM3000.cc b/motorApp/NewportSrc/drvMM3000.cc index 223961e2f..51bd3e540 100644 --- a/motorApp/NewportSrc/drvMM3000.cc +++ b/motorApp/NewportSrc/drvMM3000.cc @@ -247,7 +247,7 @@ STATIC int set_status(int card, int signal) status.All = motor_info->status.All; sprintf(outbuff, "%dMS", signal + 1); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); charcnt = recv_mess(card, inbuff, 1); if (charcnt > 0) { @@ -312,7 +312,7 @@ STATIC int set_status(int card, int signal) status.Bits.EA_HOME = 0; sprintf(outbuff, "%dTP", signal + 1); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); charcnt = recv_mess(card, inbuff, 1); if (charcnt > 0) { @@ -375,7 +375,7 @@ STATIC int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(outbuff, nodeptr->postmsgptr); - send_mess(card, outbuff, (char) NULL); + send_mess(card, outbuff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -633,7 +633,7 @@ STATIC int motor_init() /* flush any junk at input port - should not be any data available */ pasynOctetSyncIO->flush(cntrl->pasynUser); - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, axis_pos, 1); /* Return value is length of response string */ } @@ -642,12 +642,12 @@ STATIC int motor_init() { brdptr->localaddr = (char *) NULL; brdptr->motor_in_motion = 0; - send_mess(card_index, STOP_ALL, (char) NULL); /* Stop all motors */ - send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */ + send_mess(card_index, STOP_ALL, (char*) NULL); /* Stop all motors */ + send_mess(card_index, GET_IDENT, (char*) NULL); /* Read controller ID string */ recv_mess(card_index, buff, 1); strncpy(brdptr->ident, &buff[0], MAX_IDENT_LEN); /* Skip "VE" */ - send_mess(card_index, "RC", (char) NULL); + send_mess(card_index, "RC", (char*) NULL); recv_mess(card_index, buff, 1); bufptr = epicsStrtok_r(buff, "=", &tok_save); bufptr = epicsStrtok_r(NULL, " ", &tok_save); @@ -694,7 +694,7 @@ STATIC int motor_init() else { sprintf(buff, "%dTPE", motor_index + 1); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (strcmp(buff, "E01") == 0) diff --git a/motorApp/NewportSrc/drvMM4000.cc b/motorApp/NewportSrc/drvMM4000.cc index d9462f4a6..45fda5274 100644 --- a/motorApp/NewportSrc/drvMM4000.cc +++ b/motorApp/NewportSrc/drvMM4000.cc @@ -261,14 +261,14 @@ static void start_status(int card) if (card >= 0) { cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate; - send_mess(card, READ_STATUS, (char) NULL); + send_mess(card, READ_STATUS, (char*) NULL); status = recv_mess(card, cntrl->status_string, 1); if (status > 0) { cntrl->status = NORMAL; - send_mess(card, READ_POSITION, (char) NULL); + send_mess(card, READ_POSITION, (char*) NULL); recv_mess(card, cntrl->position_string, 1); - send_mess(card, READ_FEEDBACK, (char) NULL); + send_mess(card, READ_FEEDBACK, (char*) NULL); recv_mess(card, cntrl->feedback_string, 1); } else @@ -286,7 +286,7 @@ static void start_status(int card) * responses. This minimizes the latency due to processing on each card */ for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++) - send_mess(itera, READ_STATUS, (char) NULL); + send_mess(itera, READ_STATUS, (char*) NULL); for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++) { cntrl = (struct MMcontroller *) motor_state[itera]->DevicePrivate; @@ -294,7 +294,7 @@ static void start_status(int card) if (status > 0) { cntrl->status = NORMAL; - send_mess(itera, READ_FEEDBACK, (char) NULL); + send_mess(itera, READ_FEEDBACK, (char*) NULL); recv_mess(itera, cntrl->feedback_string, 1); } else @@ -306,7 +306,7 @@ static void start_status(int card) } } for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++) - send_mess(itera, READ_POSITION, (char) NULL); + send_mess(itera, READ_POSITION, (char*) NULL); for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++) { cntrl = (struct MMcontroller *) motor_state[itera]->DevicePrivate; @@ -390,7 +390,7 @@ static int set_status(int card, int signal) if (motor_info->pid_present == YES && drvMM4000ReadbackDelay != 0) { epicsThreadSleep((double) drvMM4000ReadbackDelay/1000.0); - send_mess(card, READ_STATUS, (char) NULL); + send_mess(card, READ_STATUS, (char*) NULL); recv_mess(card, cntrl->status_string, 1); pos = signal*5 + 3; /* Offset in status string */ mstat.All = cntrl->status_string[pos]; @@ -471,7 +471,7 @@ static int set_status(int card, int signal) /* Check for controller error. */ - send_mess(card, "TE;", (char) NULL); + send_mess(card, "TE;", (char*) NULL); recv_mess(card, buff, 1); if (buff[2] == '@') status.Bits.RA_PROBLEM = 0; @@ -498,7 +498,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -710,7 +710,7 @@ static int motor_init() do { - send_mess(card_index, READ_POSITION, (char) NULL); + send_mess(card_index, READ_POSITION, (char*) NULL); status = recv_mess(card_index, axis_pos, 1); retry++; /* Return value is length of response string */ @@ -721,8 +721,8 @@ static int motor_init() { brdptr->localaddr = (char *) NULL; brdptr->motor_in_motion = 0; - send_mess(card_index, STOP_ALL, (char) NULL); /* Stop all motors */ - send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */ + send_mess(card_index, STOP_ALL, (char*) NULL); /* Stop all motors */ + send_mess(card_index, GET_IDENT, (char*) NULL); /* Read controller ID string */ recv_mess(card_index, buff, 1); strcpy(brdptr->ident, &buff[2]); /* Skip "VE" */ @@ -746,7 +746,7 @@ static int motor_init() continue; } - send_mess(card_index, READ_POSITION, (char) NULL); + send_mess(card_index, READ_POSITION, (char*) NULL); recv_mess(card_index, axis_pos, 1); /* The return string will tell us how many axes this controller has */ @@ -771,7 +771,7 @@ static int motor_init() /* Determine if encoder present based on open/closed loop mode. */ sprintf(buff, "%dTC", motor_index + 1); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); loop_state = atoi(&buff[3]); /* Skip first 3 characters */ if (loop_state != 0) @@ -783,7 +783,7 @@ static int motor_init() /* Determine drive resolution. */ sprintf(buff, "%dTU", motor_index + 1); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); cntrl->drive_resolution[motor_index] = atof(&buff[3]); @@ -794,19 +794,19 @@ static int motor_init() /* Save home preset position. */ sprintf(buff, "%dXH", motor_index + 1); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); cntrl->home_preset[motor_index] = atof(&buff[3]); /* Determine low limit */ sprintf(buff, "%dTL", motor_index + 1); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); motor_info->low_limit = atof(&buff[3]); /* Determine high limit */ sprintf(buff, "%dTR", motor_index + 1); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); motor_info->high_limit = atof(&buff[3]); diff --git a/motorApp/NewportSrc/drvPM500.cc b/motorApp/NewportSrc/drvPM500.cc index 35354028f..21f452e6f 100644 --- a/motorApp/NewportSrc/drvPM500.cc +++ b/motorApp/NewportSrc/drvPM500.cc @@ -238,7 +238,7 @@ static int set_status(int card, int signal) /* Request the status and position of this motor */ sprintf(buff, "%sR", axis_name); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); rtnval = recv_mess(card, response, 1); if (rtnval > 0) { @@ -288,7 +288,7 @@ static int set_status(int card, int signal) /* Set Motor On/Off status */ sprintf(buff, "%sM?", axis_name); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); rtnval = recv_mess(card, response, 1); status.Bits.EA_POSITION = (int) atof(&response[2]); @@ -338,7 +338,7 @@ static int set_status(int card, int signal) { strcpy(buff, nodeptr->postmsgptr); strcat(buff, "\r"); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -545,7 +545,7 @@ static int motor_init() pasynOctetSyncIO->flush(cntrl->pasynUser); /* Send a SCUM 1 command to put device in this mode. */ - send_mess(card_index, "SCUM 1", (char) NULL); + send_mess(card_index, "SCUM 1", (char*) NULL); recv_mess(card_index, buff, 1); /* Set up basic controller parameters @@ -566,12 +566,12 @@ static int motor_init() * Bit 13=0, Eearly serial poll mapping * Bit 14=0, No SRQ assertion */ - send_mess(card_index, "SENAINT $AF", (char) NULL); + send_mess(card_index, "SENAINT $AF", (char*) NULL); recv_mess(card_index, buff, 1); /* Send a message and read response from controller to see if * it exists */ - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, buff, 1); /* Return value is length of response string */ } @@ -580,7 +580,7 @@ static int motor_init() { brdptr->localaddr = (char *) NULL; brdptr->motor_in_motion = 0; - send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */ + send_mess(card_index, GET_IDENT, (char*) NULL); /* Read controller ID string */ recv_mess(card_index, buff, 1); strncpy(brdptr->ident, &buff[2], 50); /* Skip "XD" */ @@ -591,7 +591,7 @@ static int motor_init() int axis_name = (int) *PM500_axis_names[total_axis]; brdptr->motor_info[total_axis].motor_motion = NULL; sprintf(buff, "%cSTAT?", axis_name); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); if (buff[1] == 'E') break; @@ -616,7 +616,7 @@ static int motor_init() double res = 0.0; sprintf(buff, "%sCONFIG?", axis_name); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); recv_mess(card_index, buff, 1); firmware = &buff[8]; Debug(3, "motor_init: firmware = %s\n", firmware); diff --git a/motorApp/OmsSrc/devOmsCom.cc b/motorApp/OmsSrc/devOmsCom.cc index 60d411076..c6bf89c20 100644 --- a/motorApp/OmsSrc/devOmsCom.cc +++ b/motorApp/OmsSrc/devOmsCom.cc @@ -265,7 +265,7 @@ RTN_STATUS oms_build_trans(motor_cmnd command, double *parms, struct motorRecord char respbuf[10]; (*tabptr->getmsg)(card, respbuf, -1); - (*tabptr->sendmsg)(card, "RB\r", (char) NULL); + (*tabptr->sendmsg)(card, "RB\r", (char*) NULL); (*tabptr->getmsg)(card, respbuf, 1); if (sscanf(respbuf, "%x", &response) == 0) response = 0; /* Force an error. */ diff --git a/motorApp/OmsSrc/drvMAXv.cc b/motorApp/OmsSrc/drvMAXv.cc index 9c5703010..c2a21d8b2 100644 --- a/motorApp/OmsSrc/drvMAXv.cc +++ b/motorApp/OmsSrc/drvMAXv.cc @@ -392,13 +392,13 @@ static int set_status(int card, int signal) MAXvCntrl = (struct MAXvController *) brdptr->DevicePrivate; if (MAXvCntrl->fwver >= 1.33) { - send_recv_mess(card, "#WS", (char) NULL, q_buf, 1); + send_recv_mess(card, "#WS", (char*) NULL, q_buf, 1); if (strcmp(q_buf, "=0") != 0) { errlogPrintf(wdctrmsg, card, q_buf); status.Bits.RA_PROBLEM = 1; motor_info->status.All = status.All; - send_mess(card, STOP_ALL, (char) NULL); + send_mess(card, STOP_ALL, (char*) NULL); /* Disable board. */ motor_state[card] = (struct controller *) NULL; return(rtn_state = 1); /* End move. */ @@ -1264,10 +1264,10 @@ static int motor_init() pmotor->status1_irq_enable.All = 0; pmotor->status2_irq_enable = 0; - send_mess(card_index, ERROR_CLEAR, (char) NULL); - send_mess(card_index, STOP_ALL, (char) NULL); + send_mess(card_index, ERROR_CLEAR, (char*) NULL); + send_mess(card_index, STOP_ALL, (char*) NULL); - send_recv_mess(card_index, GET_IDENT, (char) NULL, (char *) pmotorState->ident, 1); + send_recv_mess(card_index, GET_IDENT, (char*) NULL, (char *) pmotorState->ident, 1); Debug(3, "Identification = %s\n", pmotorState->ident); /* Save firmware version. */ @@ -1278,7 +1278,7 @@ static int motor_init() if (pvtdata->fwver >= 1.33) { - send_recv_mess(card_index, "#WS", (char) NULL, axis_pos, 1); + send_recv_mess(card_index, "#WS", (char*) NULL, axis_pos, 1); if (strcmp(axis_pos, "=0") != 0) { errlogPrintf(wdctrmsg, card_index, axis_pos); @@ -1290,9 +1290,9 @@ static int motor_init() if (wdtrip == false) { - send_mess(card_index, initstring[card_index], (char) NULL); + send_mess(card_index, initstring[card_index], (char*) NULL); - send_recv_mess(card_index, ALL_POS, (char) NULL, axis_pos, 1); + send_recv_mess(card_index, ALL_POS, (char*) NULL, axis_pos, 1); for (total_axis = 0, pos_ptr = epicsStrtok_r(axis_pos, ",", &tok_save); pos_ptr; pos_ptr = epicsStrtok_r(NULL, ",", &tok_save), total_axis++) diff --git a/motorApp/OmsSrc/drvOms.cc b/motorApp/OmsSrc/drvOms.cc index 6ee46d265..49e775cb0 100644 --- a/motorApp/OmsSrc/drvOms.cc +++ b/motorApp/OmsSrc/drvOms.cc @@ -1172,16 +1172,16 @@ static int motor_init() irqdata->irqEnable = FALSE; pmotor->control = IRQ_RESET_ID; - send_mess(card_index, "EF", (char) NULL); - send_mess(card_index, ERROR_CLEAR, (char) NULL); - send_mess(card_index, STOP_ALL, (char) NULL); + send_mess(card_index, "EF", (char*) NULL); + send_mess(card_index, ERROR_CLEAR, (char*) NULL); + send_mess(card_index, STOP_ALL, (char*) NULL); - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); recv_mess(card_index, (char *) pmotorState->ident, 1); Debug(3, "Identification = %s\n", pmotorState->ident); - send_mess(card_index, ALL_POS, (char) NULL); + send_mess(card_index, ALL_POS, (char*) NULL); recv_mess(card_index, axis_pos, 1); for (total_axis = 0, pos_ptr = epicsStrtok_r(axis_pos, ",", &tok_save); diff --git a/motorApp/OmsSrc/drvOms58.cc b/motorApp/OmsSrc/drvOms58.cc index c647211d6..d3e6780ed 100644 --- a/motorApp/OmsSrc/drvOms58.cc +++ b/motorApp/OmsSrc/drvOms58.cc @@ -1176,15 +1176,15 @@ static int motor_init() pmotor->control.cntrlReg = 0; /* Disable all interrupts */ pmotor->rebootind = 0x4321; /* Set reboot indicator (before send_mess call). */ - send_mess(card_index, "EF", (char) NULL); - send_mess(card_index, ERROR_CLEAR, (char) NULL); - send_mess(card_index, STOP_ALL, (char) NULL); + send_mess(card_index, "EF", (char*) NULL); + send_mess(card_index, ERROR_CLEAR, (char*) NULL); + send_mess(card_index, STOP_ALL, (char*) NULL); - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); recv_mess(card_index, (char *) pmotorState->ident, 1); Debug(3, "Identification = %s\n", pmotorState->ident); - send_mess(card_index, ALL_POS, (char) NULL); + send_mess(card_index, ALL_POS, (char*) NULL); recv_mess(card_index, axis_pos, 1); for (total_axis = 0, pos_ptr = epicsStrtok_r(axis_pos, ",", &tok_save); diff --git a/motorApp/OmsSrc/drvOmsPC68.cc b/motorApp/OmsSrc/drvOmsPC68.cc index 44f308922..c93bcedee 100644 --- a/motorApp/OmsSrc/drvOmsPC68.cc +++ b/motorApp/OmsSrc/drvOmsPC68.cc @@ -771,7 +771,7 @@ static int motor_init() /* Try 3 times to connect to controller. */ do { - send_mess (card_index, GET_IDENT, (char) NULL); + send_mess (card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, (char *) pmotorState->ident, 1); retry++; } while (status == 0 && retry < 3); @@ -784,11 +784,11 @@ static int motor_init() pmotorState->motor_in_motion = 0; pmotorState->cmnd_response = false; - send_mess (card_index, ECHO_OFF, (char) NULL); - send_mess (card_index, ERROR_CLEAR, (char) NULL); - send_mess (card_index, STOP_ALL, (char) NULL); + send_mess (card_index, ECHO_OFF, (char*) NULL); + send_mess (card_index, ERROR_CLEAR, (char*) NULL); + send_mess (card_index, STOP_ALL, (char*) NULL); - send_mess (card_index, ALL_POS, (char) NULL); + send_mess (card_index, ALL_POS, (char*) NULL); recv_mess (card_index, axis_pos, 1); for (total_axis = 0, pos_ptr = epicsStrtok_r(axis_pos, ",", &tok_save); @@ -823,7 +823,7 @@ static int motor_init() * dummy communication transaction. */ - send_mess (card_index, ALL_POS, (char) NULL); + send_mess (card_index, ALL_POS, (char*) NULL); recv_mess (card_index, axis_pos, 1); for (motor_index=0;motor_indexpostmsgptr != 0) { strncpy(send_buff, nodeptr->postmsgptr, 80); - send_mess(card, send_buff, (char) NULL); + send_mess(card, send_buff, (char*) NULL); nodeptr->postmsgptr = NULL; } diff --git a/motorApp/PC6KSrc/drvPC6K.cc b/motorApp/PC6KSrc/drvPC6K.cc index acc3c28c6..679f3c5dc 100644 --- a/motorApp/PC6KSrc/drvPC6K.cc +++ b/motorApp/PC6KSrc/drvPC6K.cc @@ -433,7 +433,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strncpy(send_buff, nodeptr->postmsgptr, 80); - send_mess(card, send_buff, (char) NULL); + send_mess(card, send_buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -706,12 +706,12 @@ PC6KUpLoad(int card, /* Controller Number */ /* Copy file into PC6K Program */ sprintf(nextLine, "DEL %s", progName); // recvCnt = send_recv_mess(card, nextLine, replyBuff); - send_mess(card, nextLine, (char) NULL); + send_mess(card, nextLine, (char*) NULL); // eos_ptr = eos_str; sprintf(nextLine, "DEF %s", progName); // recvCnt = send_recv_mess(card, nextLine, replyBuff, eos_ptr); // recvCnt = send_recv_mess(card, nextLine, replyBuff); - send_mess(card, nextLine, (char) NULL); + send_mess(card, nextLine, (char*) NULL); } while (fgets(nextLine, BUFF_SIZE, fd) != NULL) @@ -723,7 +723,7 @@ PC6KUpLoad(int card, /* Controller Number */ // recvCnt = send_recv_mess(card, nextLine, replyBuff, eos_ptr); // recvCnt = send_recv_mess(card, nextLine, replyBuff); - send_mess(card, nextLine, (char) NULL); + send_mess(card, nextLine, (char*) NULL); } fclose(fd); @@ -731,7 +731,7 @@ PC6KUpLoad(int card, /* Controller Number */ if (progName && strlen(progName)) /* End PC6K Program */ // recvCnt = send_recv_mess(card, "END", replyBuff); - send_mess(card, "END", (char) NULL); + send_mess(card, "END", (char*) NULL); return(OK); } diff --git a/motorApp/PiJenaSrc/drvPIJEDS.cc b/motorApp/PiJenaSrc/drvPIJEDS.cc index d559d7a54..337ce7aaa 100644 --- a/motorApp/PiJenaSrc/drvPIJEDS.cc +++ b/motorApp/PiJenaSrc/drvPIJEDS.cc @@ -392,7 +392,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -614,7 +614,7 @@ static int motor_init() { online = false; /* Set Controller to ONLINE mode */ - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); if ((status = recv_mess(card_index, buff, 1))) online = (strstr(buff,"DSM")) ? true : false; else diff --git a/motorApp/PiSrc/drvPIC662.cc b/motorApp/PiSrc/drvPIC662.cc index 1a3b175e0..b129d3a93 100644 --- a/motorApp/PiSrc/drvPIC662.cc +++ b/motorApp/PiSrc/drvPIC662.cc @@ -232,7 +232,7 @@ static int set_status(int card, int signal) nodeptr = motor_info->motor_motion; status.All = motor_info->status.All; - send_mess(card, GET_STATUS, (char) NULL); + send_mess(card, GET_STATUS, (char*) NULL); comm_status = recv_mess(card, buff, 1); if (comm_status == 0) { @@ -271,7 +271,7 @@ static int set_status(int card, int signal) * Skip to substring for this motor, convert to double */ - send_mess(card, GET_POS, (char) NULL); + send_mess(card, GET_POS, (char*) NULL); recv_mess(card, buff, 1); motorData = NINT (atof(buff) / cntrl->drive_resolution); @@ -340,7 +340,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -539,7 +539,7 @@ static int motor_init() do { - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, buff, 1); retry++; } while (status == 0 && retry < 3); @@ -558,7 +558,7 @@ static int motor_init() /* Set Controller to REMOTE mode */ - send_mess(card_index, REMOTE_MODE, (char) NULL); + send_mess(card_index, REMOTE_MODE, (char*) NULL); { struct mess_info *motor_info = &brdptr->motor_info[0]; diff --git a/motorApp/PiSrc/drvPIC663.cc b/motorApp/PiSrc/drvPIC663.cc index 666130f96..917662c15 100644 --- a/motorApp/PiSrc/drvPIC663.cc +++ b/motorApp/PiSrc/drvPIC663.cc @@ -212,7 +212,7 @@ static int set_status(int card, int signal) if (cntrl->status != NORMAL) charcnt = recv_mess(card, buff, FLUSH); - send_mess(card, "TS", (char) NULL); /* Tell Status */ + send_mess(card, "TS", (char*) NULL); /* Tell Status */ charcnt = recv_mess(card, buff, 1); if (charcnt > 9) convert_cnt = sscanf(buff, "S:%2hx %2hx %2hx\n", @@ -253,7 +253,7 @@ static int set_status(int card, int signal) minusLS = mstat2.Bits.lo_limit ? 0 : 1; /* Parse motor position */ - send_mess(card, "TP", (char) NULL); /* Tell Position */ + send_mess(card, "TP", (char*) NULL); /* Tell Position */ recv_mess(card, buff, 1); motorData = NINT(atof(&buff[2])); @@ -307,7 +307,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -519,7 +519,7 @@ static int motor_init() do { sprintf(buff,"\001%1XVE", cntrl->asyn_address); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); status = recv_mess(card_index, buff, 1); retry++; } while (status == 0 && retry < 3); diff --git a/motorApp/PiSrc/drvPIC844.cc b/motorApp/PiSrc/drvPIC844.cc index 0de16bab9..c0d0524a6 100644 --- a/motorApp/PiSrc/drvPIC844.cc +++ b/motorApp/PiSrc/drvPIC844.cc @@ -265,7 +265,7 @@ static int set_status(int card, int signal) cntrl->status = NORMAL; status.Bits.CNTRL_COMM_ERR = 0; - send_mess(card, "MOT:COND?", (char) NULL); + send_mess(card, "MOT:COND?", (char*) NULL); recv_mess(card, buff, 1); mstat.All = atoi(&buff[0]); @@ -296,7 +296,7 @@ static int set_status(int card, int signal) * Skip to substring for this motor, convert to double */ - send_mess(card, "CURR:TPOS?", (char) NULL); + send_mess(card, "CURR:TPOS?", (char*) NULL); recv_mess(card, buff, 1); motorData = atof(buff); @@ -365,7 +365,7 @@ static int set_status(int card, int signal) status.Bits.EA_SLIP_STALL = 0; status.Bits.EA_HOME = 0; - send_mess(card, "AXIS:POS?", (char) NULL); + send_mess(card, "AXIS:POS?", (char*) NULL); recv_mess(card, buff, 1); motorData = atof(buff); motor_info->encoder_position = (epicsInt32) motorData; @@ -388,7 +388,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -597,7 +597,7 @@ static int motor_init() do { - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, buff, 1); retry++; } while (status == 0 && retry < 3); diff --git a/motorApp/PiSrc/drvPIC848.cc b/motorApp/PiSrc/drvPIC848.cc index 5a8924505..d9bfd2f8d 100644 --- a/motorApp/PiSrc/drvPIC848.cc +++ b/motorApp/PiSrc/drvPIC848.cc @@ -343,7 +343,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -550,7 +550,7 @@ static int motor_init() do { - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, buff, 1); retry++; } while (status == 0 && retry < 3); diff --git a/motorApp/PiSrc/drvPIC862.cc b/motorApp/PiSrc/drvPIC862.cc index a650f4ec5..9a220d233 100644 --- a/motorApp/PiSrc/drvPIC862.cc +++ b/motorApp/PiSrc/drvPIC862.cc @@ -223,7 +223,7 @@ static int set_status(int card, int signal) if (cntrl->status != NORMAL) charcnt = recv_mess(card, buff, FLUSH); - send_mess(card, "TS", (char) NULL); /* Tell Status */ + send_mess(card, "TS", (char*) NULL); /* Tell Status */ charcnt = recv_mess(card, buff, 1); if (charcnt > 18) convert_cnt = sscanf(buff, "S:%2hx %2hx %2hx %2hx %2hx %2hx\n", @@ -279,7 +279,7 @@ static int set_status(int card, int signal) /* Parse motor position */ - send_mess(card, "TP", (char) NULL); /* Tell Position */ + send_mess(card, "TP", (char*) NULL); /* Tell Position */ recv_mess(card, buff, 1); motorData = NINT(atof(&buff[2])); @@ -333,7 +333,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -545,7 +545,7 @@ static int motor_init() do { sprintf(buff,"\001%1XVE", cntrl->asyn_address); - send_mess(card_index, buff, (char) NULL); + send_mess(card_index, buff, (char*) NULL); status = recv_mess(card_index, buff, 1); retry++; } while (status == 0 && retry < 3); diff --git a/motorApp/PiSrc/drvPIE516.cc b/motorApp/PiSrc/drvPIE516.cc index d8f8be50e..1781e4c42 100644 --- a/motorApp/PiSrc/drvPIE516.cc +++ b/motorApp/PiSrc/drvPIE516.cc @@ -244,14 +244,14 @@ static int set_status(int card, int signal) recv_mess(card, buff, FLUSH); readOK = false; - send_mess(card, READ_ONLINE, (char) NULL); + send_mess(card, READ_ONLINE, (char*) NULL); if (recv_mess(card, buff, 1) && sscanf(buff, "%d", &online_status)) { if (!online_status) { /* Assume Controller Reboot - Set ONLINE and Velocity Control ON */ - send_mess(card, SET_ONLINE, (char) NULL); - send_mess(card, SET_VELCTRL, (char) NULL); + send_mess(card, SET_ONLINE, (char*) NULL); + send_mess(card, SET_VELCTRL, (char*) NULL); } send_mess(card, READ_ONTARGET, PIE516_axis[signal]); @@ -368,7 +368,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -586,15 +586,15 @@ static int motor_init() { online = false; /* Set Controller to ONLINE mode */ - send_mess(card_index, SET_ONLINE, (char) NULL); - send_mess(card_index, READ_ONLINE, (char) NULL); + send_mess(card_index, SET_ONLINE, (char*) NULL); + send_mess(card_index, READ_ONLINE, (char*) NULL); if ((status = recv_mess(card_index, buff, 1))) online = (atoi(buff)==1) ? true : false; else retry++; } while (online == false && retry < 3); - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, buff, 1); /* Parse out E516 revision (2 decimal places) and convert to int */ @@ -627,7 +627,7 @@ static int motor_init() brdptr->total_axis = total_axis; /* Turn ON velocity control mode - All axis */ - send_mess(card_index, SET_VELCTRL, (char) NULL); + send_mess(card_index, SET_VELCTRL, (char*) NULL); for (motor_index = 0; motor_index < total_axis; motor_index++) { diff --git a/motorApp/PiSrc/drvPIE710.cc b/motorApp/PiSrc/drvPIE710.cc index 3fd920c2a..ca3275a93 100644 --- a/motorApp/PiSrc/drvPIE710.cc +++ b/motorApp/PiSrc/drvPIE710.cc @@ -352,7 +352,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -561,7 +561,7 @@ static int motor_init() do { - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, buff[0], 1); /* Parse out E710 revision (3 decimal places) and convert to int */ diff --git a/motorApp/PiSrc/drvPIE816.cc b/motorApp/PiSrc/drvPIE816.cc index 185a81b8f..f52273b62 100644 --- a/motorApp/PiSrc/drvPIE816.cc +++ b/motorApp/PiSrc/drvPIE816.cc @@ -375,7 +375,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, (char) NULL); + send_mess(card, buff, (char*) NULL); nodeptr->postmsgptr = NULL; } @@ -603,7 +603,7 @@ static int motor_init() retry++; } while (online == false && retry < 3); */ - send_mess(card_index, GET_IDENT, (char) NULL); + send_mess(card_index, GET_IDENT, (char*) NULL); status = recv_mess(card_index, buff, 1); /* Parse out E816 revision (2 decimal places) and convert to int */ diff --git a/motorApp/ThorLabsSrc/drvMDT695.cc b/motorApp/ThorLabsSrc/drvMDT695.cc index ad9c7aba1..7b80b4fda 100644 --- a/motorApp/ThorLabsSrc/drvMDT695.cc +++ b/motorApp/ThorLabsSrc/drvMDT695.cc @@ -335,7 +335,7 @@ static int set_status(int card, int signal) nodeptr->postmsgptr != 0) { strncpy(send_buff, nodeptr->postmsgptr, 80); - send_mess(card, send_buff, (char) NULL); + send_mess(card, send_buff, (char*) NULL); nodeptr->postmsgptr = NULL; } From 9368665bc208787609a9f2729ce6680d6ecf880f Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Tue, 23 Jan 2018 14:44:52 +0000 Subject: [PATCH 101/232] Reset on issuing a stop command --- motorApp/MclennanSrc/devPM304.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 125fa1431..7c5ffa75e 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -289,6 +289,10 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo of all motors */ break; case STOP_AXIS: + /* Send a reset before the stop command so that it's not impeded by a tracking abort */ + sprintf(buff, "%dRS;", axis); + strcat(motor_call->message, buff); + sprintf(buff, "%dST;", axis); break; case JOG: From c30a51a9a6e5a98aaf21dbbe0e5a3c251a12fa49 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 23 Jan 2018 15:00:26 +0000 Subject: [PATCH 102/232] white space --- motorApp/MclennanSrc/devPM304.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 7c5ffa75e..5e6d9bff3 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -231,9 +231,9 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo } if (strlen(mr->post) != 0) motor_call->postmsgptr = (char *) &mr->post; - /* Send a reset command before any move */ - if (cntrl->reset_before_move==1) { - sprintf(buff, "%dRS;", axis); + /* Send a reset command before any move */ + if (cntrl->reset_before_move==1) { + sprintf(buff, "%dRS;", axis); strcat(motor_call->message, buff); } break; @@ -289,8 +289,8 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo of all motors */ break; case STOP_AXIS: - /* Send a reset before the stop command so that it's not impeded by a tracking abort */ - sprintf(buff, "%dRS;", axis); + /* Send a reset before the stop command so that it's not impeded by a tracking abort */ + sprintf(buff, "%dRS;", axis); strcat(motor_call->message, buff); sprintf(buff, "%dST;", axis); From 2492524870a8d0cfd6111ce4b55b841b5e6de8d1 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 12 Dec 2017 17:03:55 +0000 Subject: [PATCH 103/232] Initial code to read position from the SM300 --- motorApp/Makefile | 3 + motorApp/SM300Src/Makefile | 30 ++ motorApp/SM300Src/SM300Driver.cpp | 421 +++++++++++++++++++++++++++++ motorApp/SM300Src/SM300Driver.h | 47 ++++ motorApp/SM300Src/SM300Motor.dbd | 3 + motorApp/SM300Src/SM300Register.cc | 41 +++ motorApp/SM300Src/SM300Register.h | 13 + 7 files changed, 558 insertions(+) create mode 100644 motorApp/SM300Src/Makefile create mode 100644 motorApp/SM300Src/SM300Driver.cpp create mode 100644 motorApp/SM300Src/SM300Driver.h create mode 100644 motorApp/SM300Src/SM300Motor.dbd create mode 100644 motorApp/SM300Src/SM300Register.cc create mode 100644 motorApp/SM300Src/SM300Register.h diff --git a/motorApp/Makefile b/motorApp/Makefile index 807fd6eaa..c99f8d2a8 100644 --- a/motorApp/Makefile +++ b/motorApp/Makefile @@ -105,6 +105,9 @@ MicronixSrc_DEPEND_DIRS = MotorSrc DIRS += PhytronSrc PhytronSrc_DEPEND_DIRS = MotorSrc +DIRS += SM300Src +SM300Src_DEPEND_DIRS = MotorSrc + endif # Install the edl files diff --git a/motorApp/SM300Src/Makefile b/motorApp/SM300Src/Makefile new file mode 100644 index 000000000..d49593bad --- /dev/null +++ b/motorApp/SM300Src/Makefile @@ -0,0 +1,30 @@ +# Makefile +TOP=../.. + +include $(TOP)/configure/CONFIG +#---------------------------------------- +# ADD MACRO DEFINITIONS AFTER THIS LINE +#============================= + +#================================================== +# Build an IOC support library +LIBRARY_IOC = SM300Motor + +# motorRecord.h will be created from motorRecord.dbd +# install devMotorSoft.dbd into /dbd +DBD += SM300Motor.dbd + +# The following are compiled and added to the Support library +SRCS += SM300Register.cc + +SRCS += SM300Driver.cpp + + +SM300Motor_LIBS += motor +SM300Motor_LIBS += asyn +SM300Motor_LIBS += $(EPICS_BASE_IOC_LIBS) + +include $(TOP)/configure/RULES +#---------------------------------------- +# ADD RULES AFTER THIS LINE + diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp new file mode 100644 index 000000000..9ad174a06 --- /dev/null +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -0,0 +1,421 @@ +/* +FILENAME... SM300Driver.cpp +USAGE... Motor driver support for the Newport SM300 controller. + +Based on the ACS MCB-4B Model 3 device driver written by: +Mark Rivers +March 1, 2012 + +K. Goetze 2012-03-23 Initial version + 2013-06-07 Allow motor resolution to be set using "SM300CreateController" at boot time + +*/ + + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +#include +#include "SM300Driver.h" + +#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5) + +/** Creates a new SM300Controller object. + * \param[in] portName The name of the asyn port that will be created for this driver + * \param[in] SM300PortName The name of the drvAsynSerialPort that was created previously to connect to the SM300 controller + * \param[in] numAxes The number of axes that this controller supports + * \param[in] movingPollPeriod The time between polls when any axis is moving + * \param[in] idlePollPeriod The time between polls when no axis is moving + */ +SM300Controller::SM300Controller(const char *portName, const char *SM300PortName, int numAxes, + double movingPollPeriod, double idlePollPeriod, double stepSize) + : asynMotorController(portName, numAxes, NUM_SM300_PARAMS, + 0, // No additional interfaces beyond those in base class + 0, // No additional callback interfaces beyond those in base class + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + 0, 0) // Default priority and stack size +{ + int axis; + asynStatus status; + SM300Axis *pAxis; + static const char *functionName = "SM300Controller::SM300Controller"; + + /* Connect to SM300 controller */ + status = pasynOctetSyncIO->connect(SM300PortName, 0, &pasynUserController_, NULL); + if (status) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: cannot connect to SM300 controller\n", + functionName); + } + for (axis=0; axis 0 then information is printed about each axis. + * After printing controller-specific information it calls asynMotorController::report() + */ +void SM300Controller::report(FILE *fp, int level) +{ + fprintf(fp, "SM300 motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", + this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_); + + // Call the base class method + asynMotorController::report(fp, level); +} + +/** Returns a pointer to an SM300Axis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. */ +SM300Axis* SM300Controller::getAxis(asynUser *pasynUser) +{ + return static_cast(asynMotorController::getAxis(pasynUser)); +} + +/** Returns a pointer to an SM300Axis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] No Axis index number. */ +SM300Axis* SM300Controller::getAxis(int axisNo) +{ + return static_cast(asynMotorController::getAxis(axisNo)); +} + + + + + + + + + + + +/** Polls the axis. +* This function reads motor position, limit status, home status, and moving status +* It calls setIntegerParam() and setDoubleParam() for each item that it polls, +* and then calls callParamCallbacks() at the end. +* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */ +asynStatus SM300Controller::poll() +{ + int done; + //int driveOn; + int limit; + double position; + asynStatus comStatus; + char * position_start; + SM300Axis *axis; + // Read the current motor position + sprintf(this->outString_, "LQ"); + + + comStatus = this->writeReadController(); + if (comStatus) goto skip; + + // The response string is of the form "\06\02X%d,Y%d" + position_start = &this->inString_[3]; + position = (atof(position_start) / 1.0); + //printf("Position0: %f", position); + axis = this->getAxis(0); + axis->setDoubleParam(this->motorPosition_, position); + axis->callParamCallbacks(); + axis->setIntegerParam(this->motorStatusDone_, 0); + position_start = strchr(this->inString_, ','); + + position = (atof(&position_start[2]) / 1.0); + //printf("Position1: %f", position); + axis = this->getAxis(1); + axis->setDoubleParam(this->motorPosition_, position); + axis->setIntegerParam(this->motorStatusDone_, 1); + axis->callParamCallbacks(); + + // Read the moving status of this motor +// sprintf(this->outString_, "%1dTS", axisNo_ + 1); +// comStatus = this->writeReadController(); +// if (comStatus) goto skip; + // The response string is of the form "1TS000028" + // May need to add logic for moving while homing +// done = ((this->inString_[7] == '2') && (this->inString_[8] == '8')) ? 0 : 1; + + + // Read the limit status + // The response string is of the form "1TS001328" + // + // The stage I tested this with is a GTS30V vertical jack. When the controller is initialized + // for this device using Newport's software, +25 and -5 limits get set. The controller does not + // let you set limits outside these values, so I never was able to run into a "hard" limit. + // The controller also does not allow position settings outside these limits, and does not give + // an indication. So, my recommendation is to leave the controller's travel limits set to the + // Newport defaults, and use the motor record's soft limits, set to the controller's limits or within. + // + // Should a hard limit be actually encounted, this code *should* report it to the motor record +// limit = (this->inString_[6] == '2') ? 1 : 0; +// setIntegerParam(this->motorStatusHighLimit_, limit); +// limit = (this->inString_[6] == '1') ? 1 : 0; +// setIntegerParam(this->motorStatusLowLimit_, limit); +// limit = ((this->inString_[7] == '3') && (this->inString_[8] == '2')) ? 1 : 0; +// setIntegerParam(this->motorStatusAtHome_, limit); + + // Read the drive power on status + //sprintf(this->outString_, "#%02dE", axisNo_ + 1); + //comStatus = this->writeReadController(); + //if (comStatus) goto skip; + //driveOn = (this->inString_[5] == '1') ? 1:0; + //setIntegerParam(this->motorStatusPowerOn_, driveOn); + //setIntegerParam(this->motorStatusProblem_, 0); + +skip: + axis->setIntegerParam(this->motorStatusProblem_, comStatus ? 1 : 0); + callParamCallbacks(); + return comStatus ? asynError : asynSuccess; +} + + + + + + +// These are the SM300Axis methods + +/** Creates a new SM300Axis object. + * \param[in] pC Pointer to the SM300Controller to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ +SM300Axis::SM300Axis(SM300Controller *pC, int axisNo, double stepSize) + : asynMotorAxis(pC, axisNo), + pC_(pC), stepSize_(stepSize) +{ + +} + + + +/** Reports on status of the axis + * \param[in] fp The file pointer on which report information will be written + * \param[in] level The level of report detail desired + * + * After printing device-specific information calls asynMotorAxis::report() + */ +void SM300Axis::report(FILE *fp, int level) +{ + if (level > 0) { + fprintf(fp, " axis %d\n", + axisNo_ + 1); + } + + // Call the base class method + asynMotorAxis::report(fp, level); +} + +asynStatus SM300Axis::sendAccelAndVelocity(double acceleration, double velocity) +{ + asynStatus status; + // static const char *functionName = "SM300::sendAccelAndVelocity"; + + // Send the velocity in egus + sprintf(pC_->outString_, "%1dVA%f", axisNo_ + 1, (velocity*stepSize_)); + status = pC_->writeController(); + + // Send the acceleration in egus/sec/sec + //printf("velocity: %f\n", velocity); + //printf("acceleration: %f", acceleration); + sprintf(pC_->outString_, "%1dAC%f", axisNo_ + 1, (acceleration*stepSize_)); + status = pC_->writeController(); + return status; +} + + +asynStatus SM300Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status; + // static const char *functionName = "SM300Axis::move"; + + status = sendAccelAndVelocity(acceleration, maxVelocity); + + if (relative) { + sprintf(pC_->outString_, "%1dPR%f", axisNo_ + 1, (position*stepSize_)); + } else { + sprintf(pC_->outString_, "%1dPA%f", axisNo_ + 1, (position*stepSize_)); + } + status = pC_->writeController(); + return status; +} + +asynStatus SM300Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) +{ + asynStatus status; + // static const char *functionName = "SM300Axis::home"; + + // Must be in unreferenced state to home, so can only home once after a reset + // this code should force a reset and allow a rehome, but controller doesn't seem happy + // sprintf(pC_->outString_, "%1dRS", axisNo_ + 1); + // status = pC_->writeController(); + // epicsThreadSleep(5.0); + + // set Home search velocity + //sprintf(pC_->outString_, "%1dOH%f", axisNo_ + 1, maxVelocity); + //status = pC_->writeController(); + + sprintf(pC_->outString_, "%1dOR", axisNo_ + 1); + + status = pC_->writeController(); + return status; +} + +// Jog +asynStatus SM300Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) +{ + double high_limit; + double low_limit; + asynStatus comStatus; + static const char *functionName = "SM300Axis::moveVelocity"; + + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n", + functionName, minVelocity, maxVelocity, acceleration); + + comStatus = sendAccelAndVelocity(acceleration, maxVelocity); + if (comStatus) goto skip; + + /* SM300 supports the notion of jog, but only for a remote control keypad */ + // SM300 will not allow moves outside of those set with the SL and SR commands + // first we query these limits and then make the jog a move to the limit + + // get the high limit + sprintf(pC_->outString_, "%1dSR?", axisNo_ + 1); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + // The response string is of the form "1SR25.0" + high_limit = (atof(&pC_->inString_[3])); + + // get the low limit + sprintf(pC_->outString_, "%1dSL?", axisNo_ + 1); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + // The response string is of the form "1SL-5.0" + low_limit = (atof(&pC_->inString_[3])); + + if (maxVelocity > 0.) { + /* This is a positive move in SM300 coordinates (egus) */ + sprintf(pC_->outString_, "%1dPA%f", axisNo_ + 1, high_limit); + } else { + /* This is a negative move in SM300 coordinates (egus) */ + sprintf(pC_->outString_, "%1dPA%f", axisNo_ + 1, low_limit); + } + comStatus = pC_->writeController(); + if (comStatus) goto skip; + + skip: + setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0); + callParamCallbacks(); + return comStatus ? asynError : asynSuccess; + +} + +asynStatus SM300Axis::stop(double acceleration ) +{ + asynStatus status; + //static const char *functionName = "SM300Axis::stop"; + + sprintf(pC_->outString_, "%1dST", axisNo_ + 1); + status = pC_->writeController(); + return status; +} + +asynStatus SM300Axis::setPosition(double position) +{ + asynStatus status; + //static const char *functionName = "SM300Axis::setPosition"; + + // ? not sure yet + //sprintf(pC_->outString_, "#%02dP=%+d", axisNo_ + 1, NINT(position)); + status = pC_->writeReadController(); + return status; +} + +asynStatus SM300Axis::setClosedLoop(bool closedLoop) +{ + asynStatus status; + //static const char *functionName = "SM300Axis::setClosedLoop"; + + // ? not sure yet + //sprintf(pC_->outString_, "#%02dW=%d", axisNo_ + 1, closedLoop ? 1:0); + status = pC_->writeReadController(); + return status; +} + + + +/** Code for iocsh registration */ +static const iocshArg SM300CreateControllerArg0 = {"Port name", iocshArgString}; +static const iocshArg SM300CreateControllerArg1 = {"SM300 port name", iocshArgString}; +static const iocshArg SM300CreateControllerArg2 = {"Number of axes", iocshArgInt}; +static const iocshArg SM300CreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt}; +static const iocshArg SM300CreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt}; +static const iocshArg SM300CreateControllerArg5 = {"EGUs per step", iocshArgString}; +static const iocshArg * const SM300CreateControllerArgs[] = {&SM300CreateControllerArg0, + &SM300CreateControllerArg1, + &SM300CreateControllerArg2, + &SM300CreateControllerArg3, + &SM300CreateControllerArg4, + &SM300CreateControllerArg5}; +static const iocshFuncDef SM300CreateControllerDef = {"SM300CreateController", 6, SM300CreateControllerArgs}; +static void SM300CreateContollerCallFunc(const iocshArgBuf *args) +{ + SM300CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].sval); +} + +static void SM300Register(void) +{ + iocshRegister(&SM300CreateControllerDef, SM300CreateContollerCallFunc); +} + +extern "C" { +epicsExportRegistrar(SM300Register); +} diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h new file mode 100644 index 000000000..c17b5c25c --- /dev/null +++ b/motorApp/SM300Src/SM300Driver.h @@ -0,0 +1,47 @@ +/* +FILENAME... SM300Driver.h +USAGE... Motor driver support for the SM300 controller. + +Based on the Newport SMC100 device driver written by: + +*/ + + +// No controller-specific parameters yet +#define NUM_SM300_PARAMS 0 + +class epicsShareClass SM300Axis : public asynMotorAxis +{ +public: + /* These are the methods we override from the base class */ + SM300Axis(class SM300Controller *pC, int axis, double stepSize); + void report(FILE *fp, int level); + 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); + asynStatus setClosedLoop(bool closedLoop); + +private: + SM300Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ + asynStatus sendAccelAndVelocity(double accel, double velocity); + double stepSize_; /**< Encoder increment value obtained with SU? command _or_ resolution, set at boot time */ + /* with SMC100CreateController command */ + +friend class SM300Controller; +}; + +class SM300Controller : public asynMotorController { +public: + SM300Controller(const char *portName, const char *SMC100PortName, int numAxes, double movingPollPeriod, double idlePollPeriod, double stepSize); + void report(FILE *fp, int level); + SM300Axis* getAxis(asynUser *pasynUser); + SM300Axis* getAxis(int axisNo); + + virtual asynStatus poll(); + +friend class SM300Axis; +}; diff --git a/motorApp/SM300Src/SM300Motor.dbd b/motorApp/SM300Src/SM300Motor.dbd new file mode 100644 index 000000000..39ed28453 --- /dev/null +++ b/motorApp/SM300Src/SM300Motor.dbd @@ -0,0 +1,3 @@ +# SM300 Device Driver support. +registrar(SM300Register) + diff --git a/motorApp/SM300Src/SM300Register.cc b/motorApp/SM300Src/SM300Register.cc new file mode 100644 index 000000000..a98446fa2 --- /dev/null +++ b/motorApp/SM300Src/SM300Register.cc @@ -0,0 +1,41 @@ +/* +FILENAME... SM300Register.cc +USAGE... Register SM300 motor device driver shell commands. + +Version: $Revision: 1.4 $ +Modified By: $Author: sluiter $ +Last Modified: $Date: 2004-07-16 19:06:58 $ +*/ + +/***************************************************************** + COPYRIGHT NOTIFICATION +***************************************************************** + +(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO + +This software was developed under a United States Government license +described on the COPYRIGHT_UniversityOfChicago file included as part +of this distribution. +**********************************************************************/ + +#include +#include "SM300Register.h" +#include "epicsExport.h" + +extern "C" +{ + +// SM300 Setup arguments +static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; +static const iocshArg setupArg1 = {"Polling rate", iocshArgInt}; +// SM300 Config arguments +static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; +static const iocshArg configArg1 = {"asyn port name", iocshArgString}; + +static const iocshArg * const SM300SetupArgs[2] = {&setupArg0, &setupArg1}; +static const iocshArg * const SM300ConfigArgs[2] = {&configArg0, &configArg1}; + +static const iocshFuncDef setupSM300 = {"SM300Setup", 2, SM300SetupArgs}; +static const iocshFuncDef configSM300 = {"SM300Config", 2, SM300ConfigArgs}; + +} // extern "C" diff --git a/motorApp/SM300Src/SM300Register.h b/motorApp/SM300Src/SM300Register.h new file mode 100644 index 000000000..536c70a3f --- /dev/null +++ b/motorApp/SM300Src/SM300Register.h @@ -0,0 +1,13 @@ +/* +FILENAME... SM300Register.h +USAGE... This file contains function prototypes for SM300 IOC shell commands. + +*/ + +#include "motor.h" +#include "motordrvCom.h" + +/* Function prototypes. */ +extern RTN_STATUS SM300Setup(int, int); +extern RTN_STATUS SM300Config(int, const char *); + From 8e57d88898fadbabc36b27638ba4d91f4c907cdd Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Thu, 14 Dec 2017 16:47:23 +0000 Subject: [PATCH 104/232] Add error on axis problem and tidy code --- motorApp/SM300Src/SM300Driver.cpp | 74 ++++++++++++++++++++----------- motorApp/SM300Src/SM300Driver.h | 8 ++-- 2 files changed, 54 insertions(+), 28 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 9ad174a06..485d13e1b 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -27,6 +27,7 @@ K. Goetze 2012-03-23 Initial version #include "asynMotorAxis.h" #include +#include #include "SM300Driver.h" #define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5) @@ -59,10 +60,12 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName "%s: cannot connect to SM300 controller\n", functionName); } - for (axis=0; axisoutString_, "LQ"); - comStatus = this->writeReadController(); if (comStatus) goto skip; - + // The response string is of the form "\06\02X%d,Y%d" - position_start = &this->inString_[3]; - position = (atof(position_start) / 1.0); - //printf("Position0: %f", position); + if (strlen(this->inString_) < 2) { + comStatus = asynError; + errlogPrintf("SM300 poll: status return string is too short.\n"); + goto skip; + } + + position_start = &this->inString_[1]; axis = this->getAxis(0); - axis->setDoubleParam(this->motorPosition_, position); - axis->callParamCallbacks(); - axis->setIntegerParam(this->motorStatusDone_, 0); - position_start = strchr(this->inString_, ','); + comStatus = axis->setMotorPosition(position_start); + if (comStatus) goto skip; - position = (atof(&position_start[2]) / 1.0); - //printf("Position1: %f", position); + position_start = strchr(this->inString_, ','); axis = this->getAxis(1); - axis->setDoubleParam(this->motorPosition_, position); - axis->setIntegerParam(this->motorStatusDone_, 1); - axis->callParamCallbacks(); + comStatus = axis->setMotorPosition(position_start); + if (comStatus) goto skip; // Read the moving status of this motor // sprintf(this->outString_, "%1dTS", axisNo_ + 1); @@ -180,7 +182,7 @@ asynStatus SM300Controller::poll() // The response string is of the form "1TS000028" // May need to add logic for moving while homing // done = ((this->inString_[7] == '2') && (this->inString_[8] == '8')) ? 0 : 1; - + // Read the limit status // The response string is of the form "1TS001328" @@ -209,7 +211,11 @@ asynStatus SM300Controller::poll() //setIntegerParam(this->motorStatusProblem_, 0); skip: - axis->setIntegerParam(this->motorStatusProblem_, comStatus ? 1 : 0); + for (int i; i < this->numAxes_; i++) { + axis = this->getAxis(i); + axis->setIntegerParam(this->motorStatusProblem_, comStatus ? 1 : 0); + axis->callParamCallbacks(); + } callParamCallbacks(); return comStatus ? asynError : asynSuccess; } @@ -222,18 +228,36 @@ asynStatus SM300Controller::poll() // These are the SM300Axis methods /** Creates a new SM300Axis object. - * \param[in] pC Pointer to the SM300Controller to which this axis belongs. + * \param[in] pC Pointer to the SM300Controller to which this axis belongs. * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. - * + * * Initializes register numbers, etc. */ -SM300Axis::SM300Axis(SM300Controller *pC, int axisNo, double stepSize) - : asynMotorAxis(pC, axisNo), - pC_(pC), stepSize_(stepSize) -{ +SM300Axis::SM300Axis(SM300Controller *pC, int axisNo, char axisLabel) + : asynMotorAxis(pC, axisNo), + pC_(pC), axisLabel(axisLabel) +{ } +/** Set the known motor position from the return string. + * \param(in) lqReply the reply to LQ status request, should be a character followed by the motor axis then an integer + */ +asynStatus SM300Axis::setMotorPosition(const char * lqReply) +{ + char *stop_char; + if ((lqReply == NULL) || (strlen(lqReply) < 3) || (lqReply[1] != axisLabel)) { + errlogPrintf("SM300 poll: LQ reply does not contain position for 2 motors.\n"); + return asynError; + } + double position = (strtod(&lqReply[2], &stop_char) / 1.0); + if (stop_char[0]!='\0' && stop_char[0]!=',') { + errlogPrintf("SM300 poll: LQ reply contains non number %s.\n", &lqReply[2]); + return asynError; + } + setDoubleParam(pC_->motorPosition_, position); + return asynSuccess; +} /** Reports on status of the axis diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index c17b5c25c..a2b5ccaa0 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -14,7 +14,7 @@ class epicsShareClass SM300Axis : public asynMotorAxis { public: /* These are the methods we override from the base class */ - SM300Axis(class SM300Controller *pC, int axis, double stepSize); + SM300Axis(class SM300Controller *pC, int axis, char axisLabel); void report(FILE *fp, int level); asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); @@ -30,7 +30,8 @@ class epicsShareClass SM300Axis : public asynMotorAxis asynStatus sendAccelAndVelocity(double accel, double velocity); double stepSize_; /**< Encoder increment value obtained with SU? command _or_ resolution, set at boot time */ /* with SMC100CreateController command */ - + char axisLabel; /** label for the axis*/ + asynStatus setMotorPosition(const char * lq_return); friend class SM300Controller; }; @@ -41,7 +42,8 @@ class SM300Controller : public asynMotorController { SM300Axis* getAxis(asynUser *pasynUser); SM300Axis* getAxis(int axisNo); - virtual asynStatus poll(); + virtual asynStatus poll(); friend class SM300Axis; + }; From f0805d100a0651d4af0bb1578a4e3d03711e56ae Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Mon, 18 Dec 2017 16:55:18 +0000 Subject: [PATCH 105/232] add movement status --- motorApp/SM300Src/SM300Driver.cpp | 76 +++++++++++++++---------------- motorApp/SM300Src/SM300Driver.h | 3 ++ 2 files changed, 41 insertions(+), 38 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 485d13e1b..bcf53ad69 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -41,7 +41,8 @@ K. Goetze 2012-03-23 Initial version */ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName, int numAxes, double movingPollPeriod, double idlePollPeriod, double stepSize) - : asynMotorController(portName, numAxes, NUM_SM300_PARAMS, + : _status_set(0), + asynMotorController(portName, numAxes, NUM_SM300_PARAMS, 0, // No additional interfaces beyond those in base class 0, // No additional callback interfaces beyond those in base class ASYN_CANBLOCK | ASYN_MULTIDEVICE, @@ -64,9 +65,10 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName //for (axis=1; axis < (numAxes + 1); axis++) { // pAxis = new SM300Axis(this, axis, stepSize); // } - pAxis = new SM300Axis(this, 0, 'X'); + pAxis = new SM300Axis(this, 0, 'X'); pAxis = new SM300Axis(this, 1, 'Y'); + startPoller(movingPollPeriod, idlePollPeriod, 2); } @@ -175,43 +177,42 @@ asynStatus SM300Controller::poll() comStatus = axis->setMotorPosition(position_start); if (comStatus) goto skip; - // Read the moving status of this motor -// sprintf(this->outString_, "%1dTS", axisNo_ + 1); -// comStatus = this->writeReadController(); -// if (comStatus) goto skip; - // The response string is of the form "1TS000028" - // May need to add logic for moving while homing -// done = ((this->inString_[7] == '2') && (this->inString_[8] == '8')) ? 0 : 1; - - - // Read the limit status - // The response string is of the form "1TS001328" - // - // The stage I tested this with is a GTS30V vertical jack. When the controller is initialized - // for this device using Newport's software, +25 and -5 limits get set. The controller does not - // let you set limits outside these values, so I never was able to run into a "hard" limit. - // The controller also does not allow position settings outside these limits, and does not give - // an indication. So, my recommendation is to leave the controller's travel limits set to the - // Newport defaults, and use the motor record's soft limits, set to the controller's limits or within. - // - // Should a hard limit be actually encounted, this code *should* report it to the motor record -// limit = (this->inString_[6] == '2') ? 1 : 0; -// setIntegerParam(this->motorStatusHighLimit_, limit); -// limit = (this->inString_[6] == '1') ? 1 : 0; -// setIntegerParam(this->motorStatusLowLimit_, limit); -// limit = ((this->inString_[7] == '3') && (this->inString_[8] == '2')) ? 1 : 0; -// setIntegerParam(this->motorStatusAtHome_, limit); - - // Read the drive power on status - //sprintf(this->outString_, "#%02dE", axisNo_ + 1); - //comStatus = this->writeReadController(); - //if (comStatus) goto skip; - //driveOn = (this->inString_[5] == '1') ? 1:0; - //setIntegerParam(this->motorStatusPowerOn_, driveOn); - //setIntegerParam(this->motorStatusProblem_, 0); + // Read the current motor position + sprintf(this->outString_, "LM"); + + comStatus = this->writeReadController(); + if (comStatus) goto skip; + + if (strlen(this->inString_) < 3) { + comStatus = asynError; + errlogPrintf("SM300 poll: moving status return string is too short.\n"); + goto skip; + } + errlogPrintf("SM300 poll: moving status returned string is %s.\n", this->inString_); + + bool done_moving; + if (this->inString_[2] == 'P') { + done_moving = true; + } + else if (this->inString_[2] == 'N') { + done_moving = false; + } + else { + comStatus = asynError; + errlogPrintf("SM300 poll: moving status returned error.\n"); + goto skip; + } + for (int i=0; i < this->numAxes_; i++) { + axis = this->getAxis(i); + // Driver appears to ignore the first and second change in status so instead make sure change is registered + if (_status_set < 3) axis->setIntegerParam(motorStatusDone_, done_moving ? 0 : 1); + axis->setIntegerParam(motorStatusDone_, done_moving ? 1 : 0); + errlogPrintf("SM300 poll: set axis status done %i to %i.\n", i, done_moving ? 1 : 0); + } + _status_set ++; skip: - for (int i; i < this->numAxes_; i++) { + for (int i=0; i < this->numAxes_; i++) { axis = this->getAxis(i); axis->setIntegerParam(this->motorStatusProblem_, comStatus ? 1 : 0); axis->callParamCallbacks(); @@ -237,7 +238,6 @@ SM300Axis::SM300Axis(SM300Controller *pC, int axisNo, char axisLabel) : asynMotorAxis(pC, axisNo), pC_(pC), axisLabel(axisLabel) { - } /** Set the known motor position from the return string. diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index a2b5ccaa0..844f23544 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -44,6 +44,9 @@ class SM300Controller : public asynMotorController { virtual asynStatus poll(); +private: + int _status_set; + friend class SM300Axis; }; From dcaf5fd536504d66e6d445c6966dc73c755ba785 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Wed, 20 Dec 2017 14:18:07 +0000 Subject: [PATCH 106/232] Readand write in motor steps --- motorApp/SM300Src/SM300Driver.cpp | 162 ++++++++++++++++-------------- motorApp/SM300Src/SM300Driver.h | 10 +- 2 files changed, 90 insertions(+), 82 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index bcf53ad69..026addf28 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -41,19 +41,17 @@ K. Goetze 2012-03-23 Initial version */ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName, int numAxes, double movingPollPeriod, double idlePollPeriod, double stepSize) - : _status_set(0), - asynMotorController(portName, numAxes, NUM_SM300_PARAMS, + : asynMotorController(portName, numAxes, NUM_SM300_PARAMS, 0, // No additional interfaces beyond those in base class 0, // No additional callback interfaces beyond those in base class ASYN_CANBLOCK | ASYN_MULTIDEVICE, 1, // autoconnect 0, 0) // Default priority and stack size { - int axis; asynStatus status; SM300Axis *pAxis; static const char *functionName = "SM300Controller::SM300Controller"; - + /* Connect to SM300 controller */ status = pasynOctetSyncIO->connect(SM300PortName, 0, &pasynUserController_, NULL); if (status) { @@ -61,14 +59,12 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName "%s: cannot connect to SM300 controller\n", functionName); } -// for (axis=0; axis(asynMotorController::getAxis(axisNo)); } +/** Set the termination characters on the output and input buffers + * \param[in] eosIn end of string for input + * \param[in] eosInlen length of end of string for input + * \param[in] eosOut end of string for output + * \param[in] eosOutlen length of end of string for output + */ +void SM300Controller::setTerminationChars(const char *eosIn, int eosInlen, const char *eosOut, int eosOutlen) { - - - - - - - - + pasynOctetSyncIO->setOutputEos(this->pasynUserController_, eosOut, eosOutlen); + pasynOctetSyncIO->setInputEos(this->pasynUserController_, eosIn, eosInlen); +} /** Polls the axis. -* This function reads motor position, limit status, home status, and moving status -* It calls setIntegerParam() and setDoubleParam() for each item that it polls, -* and then calls callParamCallbacks() at the end. -* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */ + * This function reads moving status which is done on a controller + */ asynStatus SM300Controller::poll() { - int done; - //int driveOn; - int limit; - double position; + asynStatus comStatus; - char * position_start; SM300Axis *axis; - // Read the current motor position - sprintf(this->outString_, "LQ"); - - comStatus = this->writeReadController(); - if (comStatus) goto skip; - - // The response string is of the form "\06\02X%d,Y%d" - if (strlen(this->inString_) < 2) { - comStatus = asynError; - errlogPrintf("SM300 poll: status return string is too short.\n"); - goto skip; - } - position_start = &this->inString_[1]; - axis = this->getAxis(0); - comStatus = axis->setMotorPosition(position_start); - if (comStatus) goto skip; - - position_start = strchr(this->inString_, ','); - axis = this->getAxis(1); - comStatus = axis->setMotorPosition(position_start); - if (comStatus) goto skip; + // Read the current motor position + setTerminationChars("\x04", 1, "\x04", 1); // Read the current motor position sprintf(this->outString_, "LM"); @@ -188,7 +161,6 @@ asynStatus SM300Controller::poll() errlogPrintf("SM300 poll: moving status return string is too short.\n"); goto skip; } - errlogPrintf("SM300 poll: moving status returned string is %s.\n", this->inString_); bool done_moving; if (this->inString_[2] == 'P') { @@ -199,31 +171,31 @@ asynStatus SM300Controller::poll() } else { comStatus = asynError; - errlogPrintf("SM300 poll: moving status returned error.\n"); + errlogPrintf("SM300 poll: moving status returned error status.\n"); goto skip; } for (int i=0; i < this->numAxes_; i++) { axis = this->getAxis(i); - // Driver appears to ignore the first and second change in status so instead make sure change is registered - if (_status_set < 3) axis->setIntegerParam(motorStatusDone_, done_moving ? 0 : 1); axis->setIntegerParam(motorStatusDone_, done_moving ? 1 : 0); - errlogPrintf("SM300 poll: set axis status done %i to %i.\n", i, done_moving ? 1 : 0); } - _status_set ++; skip: + has_error_ = comStatus != asynSuccess; + for (int i=0; i < this->numAxes_; i++) { axis = this->getAxis(i); - axis->setIntegerParam(this->motorStatusProblem_, comStatus ? 1 : 0); + axis->setIntegerParam(this->motorStatusProblem_, axis->has_error() || has_error_ ? 1 : 0); axis->callParamCallbacks(); } callParamCallbacks(); return comStatus ? asynError : asynSuccess; } - - - +/** Get an whether the controller has an error. +*/ +bool SM300Controller::has_error() { + return has_error_; +} // These are the SM300Axis methods @@ -240,23 +212,49 @@ SM300Axis::SM300Axis(SM300Controller *pC, int axisNo, char axisLabel) { } -/** Set the known motor position from the return string. - * \param(in) lqReply the reply to LQ status request, should be a character followed by the motor axis then an integer - */ -asynStatus SM300Axis::setMotorPosition(const char * lqReply) +/** Get an whether the axis has an error. +*/ +bool SM300Axis::has_error() { + return has_error_; +} + +/** Polls the axis. +* This function reads the position of the axis +* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). Not used as this is reported at the controller level +*/ +asynStatus SM300Axis::poll(bool *moving) { - char *stop_char; - if ((lqReply == NULL) || (strlen(lqReply) < 3) || (lqReply[1] != axisLabel)) { - errlogPrintf("SM300 poll: LQ reply does not contain position for 2 motors.\n"); - return asynError; + double position; + asynStatus comStatus; + + // Read the current motor position + pC_->setTerminationChars("\x04", 1, "\x04", 1); + sprintf(pC_->outString_, "LI%c", this->axisLabel); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + + // The response string is of the form "\06\02%d" + if (strlen(pC_->inString_) < 3) { + comStatus = asynError; + errlogPrintf("SM300 poll: position return string is too short.\n"); + goto skip; } - double position = (strtod(&lqReply[2], &stop_char) / 1.0); - if (stop_char[0]!='\0' && stop_char[0]!=',') { - errlogPrintf("SM300 poll: LQ reply contains non number %s.\n", &lqReply[2]); - return asynError; + + + char *stop_char; + position = (strtod(&pC_->inString_[2], &stop_char) / 1.0); + if (stop_char[0] != '\0') { + errlogPrintf("SM300 poll: LI reply contains non number %s.\n", &pC_->inString_[2]); + comStatus = asynError; + goto skip; } setDoubleParam(pC_->motorPosition_, position); - return asynSuccess; + +skip: + has_error_ = comStatus != asynSuccess; + setIntegerParam(pC_->motorStatusProblem_, has_error_ || pC_->has_error() ? 1 : 0); + callParamCallbacks(); + return comStatus ? asynError : asynSuccess; } @@ -294,20 +292,28 @@ asynStatus SM300Axis::sendAccelAndVelocity(double acceleration, double velocity) return status; } - +/** Move the motor to an absolute location or by a relative amount (uses current location since the motor doesw not support a relative command). +* \param[in] position The absolute position to move to (if relative=0) or the relative distance to move +* by (if relative=1). Units=steps. +* \param[in] relative Flag indicating relative move (1) or absolute move (0). +* \param[in] minVelocity The initial velocity, often called the base velocity. Units=steps/sec. +* \param[in] maxVelocity The maximum velocity, often called the slew velocity. Units=steps/sec. +* \param[in] acceleration The acceleration value. Units=steps/sec/sec. */ asynStatus SM300Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { asynStatus status; - // static const char *functionName = "SM300Axis::move"; - status = sendAccelAndVelocity(acceleration, maxVelocity); + //status = sendAccelAndVelocity(acceleration, maxVelocity); - if (relative) { + if (relative==1) { //relative move sprintf(pC_->outString_, "%1dPR%f", axisNo_ + 1, (position*stepSize_)); - } else { - sprintf(pC_->outString_, "%1dPA%f", axisNo_ + 1, (position*stepSize_)); - } - status = pC_->writeController(); + } + + sprintf(pC_->outString_, "B%c%.0f", axisLabel, round(position)); + + pC_->setTerminationChars("\06", 1, "\04", 1); + status = pC_->writeReadController(); + return status; } diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 844f23544..881b7bc98 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -20,10 +20,10 @@ class epicsShareClass SM300Axis : public asynMotorAxis 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 poll(bool *moving); asynStatus setPosition(double position); asynStatus setClosedLoop(bool closedLoop); - + bool has_error(); private: SM300Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. * Abbreviated because it is used very frequently */ @@ -31,7 +31,7 @@ class epicsShareClass SM300Axis : public asynMotorAxis double stepSize_; /**< Encoder increment value obtained with SU? command _or_ resolution, set at boot time */ /* with SMC100CreateController command */ char axisLabel; /** label for the axis*/ - asynStatus setMotorPosition(const char * lq_return); + bool has_error_; friend class SM300Controller; }; @@ -41,11 +41,13 @@ class SM300Controller : public asynMotorController { void report(FILE *fp, int level); SM300Axis* getAxis(asynUser *pasynUser); SM300Axis* getAxis(int axisNo); + void setTerminationChars(const char *eosIn, int eosInlen, const char *eosOut, int eosOutlen); virtual asynStatus poll(); + bool has_error(); private: - int _status_set; + bool has_error_; friend class SM300Axis; From 5a0068c1774dda6d6717c01d0713b8f6db94a407 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Wed, 20 Dec 2017 15:50:21 +0000 Subject: [PATCH 107/232] Add homing --- motorApp/SM300Src/SM300Driver.cpp | 23 ++++++++--------------- 1 file changed, 8 insertions(+), 15 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 026addf28..389c90ef0 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -302,14 +302,16 @@ asynStatus SM300Axis::sendAccelAndVelocity(double acceleration, double velocity) asynStatus SM300Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { asynStatus status; + double move_to = position; //status = sendAccelAndVelocity(acceleration, maxVelocity); if (relative==1) { //relative move - sprintf(pC_->outString_, "%1dPR%f", axisNo_ + 1, (position*stepSize_)); + errlogPrintf("SM300 move: Can not do relative move with this motor.\n"); + return asynError; } - sprintf(pC_->outString_, "B%c%.0f", axisLabel, round(position)); + sprintf(pC_->outString_, "B%c%.0f", axisLabel, round(move_to)); pC_->setTerminationChars("\06", 1, "\04", 1); status = pC_->writeReadController(); @@ -320,21 +322,12 @@ asynStatus SM300Axis::move(double position, int relative, double minVelocity, do asynStatus SM300Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) { asynStatus status; - // static const char *functionName = "SM300Axis::home"; - - // Must be in unreferenced state to home, so can only home once after a reset - // this code should force a reset and allow a rehome, but controller doesn't seem happy - // sprintf(pC_->outString_, "%1dRS", axisNo_ + 1); - // status = pC_->writeController(); - // epicsThreadSleep(5.0); - // set Home search velocity - //sprintf(pC_->outString_, "%1dOH%f", axisNo_ + 1, maxVelocity); - //status = pC_->writeController(); + sprintf(pC_->outString_, "BR%c", axisLabel); + + pC_->setTerminationChars("\06", 1, "\04", 1); + status = pC_->writeReadController(); - sprintf(pC_->outString_, "%1dOR", axisNo_ + 1); - - status = pC_->writeController(); return status; } From 1df0e10893350d312b96d6a0dcef40b27f8912a3 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Fri, 22 Dec 2017 16:05:12 +0000 Subject: [PATCH 108/232] Add reset functionality --- motorApp/Db/Makefile | 2 +- motorApp/Db/SM300_extra.db | 11 +++ motorApp/SM300Src/SM300Driver.cpp | 142 ++++++++++++++++++++++++------ motorApp/SM300Src/SM300Driver.h | 10 ++- 4 files changed, 136 insertions(+), 29 deletions(-) create mode 100644 motorApp/Db/SM300_extra.db diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index abbe6a02b..633c971fb 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -46,7 +46,7 @@ DB += profileMoveAxisXPS.template DB += PI_Support.db PI_SupportCtrl.db DB += Phytron_motor.db Phytron_I1AM01.db Phytron_MCM01.db DB += linmot_extra.db - +DB += SM300_extra.db #---------------------------------------------------- # Declare template files which do not show up in DB #USES_TEMPLATE += motor.template diff --git a/motorApp/Db/SM300_extra.db b/motorApp/Db/SM300_extra.db new file mode 100644 index 000000000..15d8840be --- /dev/null +++ b/motorApp/Db/SM300_extra.db @@ -0,0 +1,11 @@ +# Database for SM300 + +record(bo,"$(P)RESET") { +field(DTYP, "asynInt32") + field(DESC,"Reset the motor parameters") + + field(ZNAM, "Done") + field(ONAM, "Reseting") + field(VAL, "0") + field(OUT, "@asyn($(PORT))RESET") +} diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 389c90ef0..144bb87cc 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -65,9 +65,113 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName pAxis = new SM300Axis(this, 0, 'X'); pAxis = new SM300Axis(this, 1, 'Y'); + + createParam(SM300ResetString, asynParamInt32, &reset_); startPoller(movingPollPeriod, idlePollPeriod, 2); } +asynStatus SM300Controller::sendQuerry(const char * querry) { + setTerminationChars("\x04", 1, "\x04", 1); + //send data format 2 + sprintf(this->outString_, "\x06\x02%s", querry); + return this->writeReadController(); +} + + +asynStatus SM300Controller::sendCommand(const char * querry) { + setTerminationChars("\x06", 1, "\x04", 1); + //send data format 2 + sprintf(this->outString_, "\x06\x02%s", querry); + return this->writeReadController(); +} + + +asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { + asynStatus status; + + int function = pasynUser->reason; //Function requested + if (function == reset_) { + if (value == 0) return asynSuccess; + setTerminationChars("\x04", 1, "\x04", 1); + + //send empty string with ACK to clear the buffer + status = this->writeController(); + + //set termination character is EOT without check sum * + // when sedning send incase this mode is switched on + setTerminationChars("\x06", 1, "\x04\x0D", 2); + if (sendCommand("PEK0")) return status; + + + setTerminationChars("\x06", 1, "\x04", 1); + // set achknowlegement on + if (sendCommand("PEL1")) return status; + // set linear interpolation + if (sendCommand("B/ G01")) return status; + // absoulute coordinates + if (sendCommand("B/ G90")) return status; + // switch off message from contrl unit when motor is in position or in error (will poll instead) + if (sendCommand("PER0")) return status; + + //send data format 2 - 100s + if (sendCommand("PXA2")) return status; + if (sendCommand("PYA2")) return status; + // Gear factor numerator + if (sendCommand("PXB5")) return status; + if (sendCommand("PYB1")) return status; + // Gear factor denomenator + if (sendCommand("PXC10")) return status; + if (sendCommand("PYC10")) return status; + // Drag Error + if (sendCommand("PXD2500")) return status; + if (sendCommand("PYD2500")) return status; + // Start/stop ramp + if (sendCommand("PXE100000")) return status; + if (sendCommand("PYE25000")) return status; + // KV factor oe feedback control amplification + if (sendCommand("PXF1000")) return status; + if (sendCommand("PYF1000")) return status; + // Regulator factor A + if (sendCommand("PXG0")) return status; + if (sendCommand("PYG0")) return status; + // +Software switch limit + if (sendCommand("PXH+57000")) return status; + if (sendCommand("PYH+64000")) return status; + // -Software switch limit + if (sendCommand("PXI-50")) return status; + if (sendCommand("PYI-20")) return status; + // maximum speed + if (sendCommand("PXJ25000")) return status; + if (sendCommand("PYJ25000")) return status; + // direction and speed of homing procedure + if (sendCommand("PXK-2500")) return status; + if (sendCommand("PYK-7500")) return status; + // standstill check + if (sendCommand("PXL100")) return status; + if (sendCommand("PYL100")) return status; + // Inposition window + if (sendCommand("PXM5")) return status; + if (sendCommand("PYM5")) return status; + // Distance from reference switch + if (sendCommand("PXN1000")) return status; + if (sendCommand("PYN5000")) return status; + // Backlash compensation + if (sendCommand("PXO0")) return status; + if (sendCommand("PYO0")) return status; + // Moving direction + if (sendCommand("PXP0")) return status; + if (sendCommand("PYP0")) return status; + // Feed for axes + if (sendCommand("BF15000")) return status; + //TODO Do I need a PR? + + } + else { + status = asynMotorController::writeInt32(pasynUser, value); + } + return status; +} + /** Creates a new SM300Controller object. * Configuration command, called directly or from iocsh @@ -147,13 +251,8 @@ asynStatus SM300Controller::poll() asynStatus comStatus; SM300Axis *axis; - // Read the current motor position - setTerminationChars("\x04", 1, "\x04", 1); - - // Read the current motor position - sprintf(this->outString_, "LM"); - - comStatus = this->writeReadController(); + // Read the current status of the motor (at Poition, Not at position, Error) + comStatus = this->sendQuerry("LM"); if (comStatus) goto skip; if (strlen(this->inString_) < 3) { @@ -224,13 +323,13 @@ bool SM300Axis::has_error() { */ asynStatus SM300Axis::poll(bool *moving) { + char temp[MAX_CONTROLLER_STRING_SIZE]; double position; asynStatus comStatus; - // Read the current motor position - pC_->setTerminationChars("\x04", 1, "\x04", 1); - sprintf(pC_->outString_, "LI%c", this->axisLabel); - comStatus = pC_->writeReadController(); + // Read the current motor position + sprintf(temp, "LI%c", this->axisLabel); + comStatus = pC_->sendQuerry(temp); if (comStatus) goto skip; // The response string is of the form "\06\02%d" @@ -301,7 +400,7 @@ asynStatus SM300Axis::sendAccelAndVelocity(double acceleration, double velocity) * \param[in] acceleration The acceleration value. Units=steps/sec/sec. */ asynStatus SM300Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { - asynStatus status; + char temp[MAX_CONTROLLER_STRING_SIZE]; double move_to = position; //status = sendAccelAndVelocity(acceleration, maxVelocity); @@ -310,25 +409,16 @@ asynStatus SM300Axis::move(double position, int relative, double minVelocity, do errlogPrintf("SM300 move: Can not do relative move with this motor.\n"); return asynError; } - - sprintf(pC_->outString_, "B%c%.0f", axisLabel, round(move_to)); - pC_->setTerminationChars("\06", 1, "\04", 1); - status = pC_->writeReadController(); - - return status; + sprintf(temp, "B%c%.0f", axisLabel, round(move_to)); + return pC_->sendCommand(temp); } asynStatus SM300Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) { - asynStatus status; - - sprintf(pC_->outString_, "BR%c", axisLabel); - - pC_->setTerminationChars("\06", 1, "\04", 1); - status = pC_->writeReadController(); - - return status; + char temp[MAX_CONTROLLER_STRING_SIZE]; + sprintf(temp, "BR%c", axisLabel); + return pC_->sendCommand(temp); } // Jog diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 881b7bc98..97fd38442 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -7,8 +7,9 @@ Based on the Newport SMC100 device driver written by: */ -// No controller-specific parameters yet -#define NUM_SM300_PARAMS 0 +// controller-specific parameters yet +#define NUM_SM300_PARAMS 1 +#define SM300ResetString "RESET" class epicsShareClass SM300Axis : public asynMotorAxis { @@ -43,11 +44,16 @@ class SM300Controller : public asynMotorController { SM300Axis* getAxis(int axisNo); void setTerminationChars(const char *eosIn, int eosInlen, const char *eosOut, int eosOutlen); + virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + virtual asynStatus poll(); + asynStatus sendCommand(const char * querry); + asynStatus sendQuerry(const char * querry); bool has_error(); private: bool has_error_; + int reset_; friend class SM300Axis; From c1afb1022f8a53d4a64517d5fd29e3528e3b5ed6 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 2 Jan 2018 13:21:29 +0000 Subject: [PATCH 109/232] Add stop --- motorApp/SM300Src/SM300Driver.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 144bb87cc..1edce10f7 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -474,11 +474,10 @@ asynStatus SM300Axis::moveVelocity(double minVelocity, double maxVelocity, doubl asynStatus SM300Axis::stop(double acceleration ) { asynStatus status; - //static const char *functionName = "SM300Axis::stop"; - sprintf(pC_->outString_, "%1dST", axisNo_ + 1); - status = pC_->writeController(); - return status; + status = pC_->sendQuerry("BSS"); + // ignore reply it has to be P + return status ? asynError : asynSuccess; } asynStatus SM300Axis::setPosition(double position) From b4872e037f481433ee42a7e293717354e2618391 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 2 Jan 2018 14:03:02 +0000 Subject: [PATCH 110/232] Add global moving and make polling quicker when moving --- motorApp/Db/motorUtil.db | 3 +++ motorApp/SM300Src/SM300Driver.cpp | 20 ++++++++++++++------ motorApp/SM300Src/SM300Driver.h | 2 ++ 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 1115804ed..233b5c8ea 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -53,6 +53,8 @@ record(calcout, "$(P)_MOVING") ## specific IOC and no two lines will get loaded at one. However ## it is IMPORTANT to not re-use one of the outputs on the ## MOVING or _MOVING calc record target +## galils done in a slightly different way for historical reasons, +## will be fixed in upcoming ticket. record(dfanout, "$(P)_FAN") { field(OMSL, "supervisory") @@ -77,6 +79,7 @@ $(IFIOC_LINMOT_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.G CA") $(IFIOC_LINMOT_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.H CA") $(IFIOC_BKHOFF_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.I CA") $(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") +$(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") } # force periodic processing to catch up any missing, shouldn't diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 1edce10f7..2a00099e0 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -41,7 +41,8 @@ K. Goetze 2012-03-23 Initial version */ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName, int numAxes, double movingPollPeriod, double idlePollPeriod, double stepSize) - : asynMotorController(portName, numAxes, NUM_SM300_PARAMS, + : is_moving_(false), + asynMotorController(portName, numAxes, NUM_SM300_PARAMS, 0, // No additional interfaces beyond those in base class 0, // No additional callback interfaces beyond those in base class ASYN_CANBLOCK | ASYN_MULTIDEVICE, @@ -85,6 +86,12 @@ asynStatus SM300Controller::sendCommand(const char * querry) { return this->writeReadController(); } +/** + * Return true if the controller registering moving motors + */ +bool SM300Controller::is_moving() { + return is_moving_; +} asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { asynStatus status; @@ -261,21 +268,21 @@ asynStatus SM300Controller::poll() goto skip; } - bool done_moving; if (this->inString_[2] == 'P') { - done_moving = true; + is_moving_ = false; } else if (this->inString_[2] == 'N') { - done_moving = false; + is_moving_ = true; } else { + is_moving_ = false; comStatus = asynError; errlogPrintf("SM300 poll: moving status returned error status.\n"); goto skip; } for (int i=0; i < this->numAxes_; i++) { axis = this->getAxis(i); - axis->setIntegerParam(motorStatusDone_, done_moving ? 1 : 0); + axis->setIntegerParam(motorStatusDone_, is_moving_ ? 0 : 1); } skip: @@ -327,6 +334,8 @@ asynStatus SM300Axis::poll(bool *moving) double position; asynStatus comStatus; + *moving = pC_->is_moving(); + // Read the current motor position sprintf(temp, "LI%c", this->axisLabel); comStatus = pC_->sendQuerry(temp); @@ -339,7 +348,6 @@ asynStatus SM300Axis::poll(bool *moving) goto skip; } - char *stop_char; position = (strtod(&pC_->inString_[2], &stop_char) / 1.0); if (stop_char[0] != '\0') { diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 97fd38442..9d6a4d501 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -50,9 +50,11 @@ class SM300Controller : public asynMotorController { asynStatus sendCommand(const char * querry); asynStatus sendQuerry(const char * querry); bool has_error(); + bool is_moving(); private: bool has_error_; + bool is_moving_; int reset_; friend class SM300Axis; From dba9f42ade95d8ee4519c94c8a47ad2bf7eeda7e Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 2 Jan 2018 15:17:26 +0000 Subject: [PATCH 111/232] Make reset a busy record and tidy --- motorApp/Db/SM300_extra.db | 2 +- motorApp/SM300Src/SM300Driver.cpp | 132 ++++++++---------------------- motorApp/SM300Src/SM300Driver.h | 8 +- motorApp/SM300Src/SM300Register.h | 3 +- 4 files changed, 38 insertions(+), 107 deletions(-) diff --git a/motorApp/Db/SM300_extra.db b/motorApp/Db/SM300_extra.db index 15d8840be..0fd19e56c 100644 --- a/motorApp/Db/SM300_extra.db +++ b/motorApp/Db/SM300_extra.db @@ -1,6 +1,6 @@ # Database for SM300 -record(bo,"$(P)RESET") { +record(busy,"$(P)RESET") { field(DTYP, "asynInt32") field(DESC,"Reset the motor parameters") diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 2a00099e0..161d279f9 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -1,14 +1,7 @@ /* -FILENAME... SM300Driver.cpp -USAGE... Motor driver support for the Newport SM300 controller. - -Based on the ACS MCB-4B Model 3 device driver written by: -Mark Rivers -March 1, 2012 - -K. Goetze 2012-03-23 Initial version - 2013-06-07 Allow motor resolution to be set using "SM300CreateController" at boot time +Motor driver support for the SM300 controller. +Based on the SM100 Model 3 device driver */ @@ -71,6 +64,12 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName startPoller(movingPollPeriod, idlePollPeriod, 2); } +/** Send a querry string to the controller and get a return value. + * Querry string is prefeced with ACK STX and postfixed with EOT + * return string ends with ETX + * \param[in] querry the querry to send + * \returns success of write and read of querry string + */ asynStatus SM300Controller::sendQuerry(const char * querry) { setTerminationChars("\x04", 1, "\x04", 1); //send data format 2 @@ -78,11 +77,15 @@ asynStatus SM300Controller::sendQuerry(const char * querry) { return this->writeReadController(); } - -asynStatus SM300Controller::sendCommand(const char * querry) { +/** Send a command string to the controller. +* Command string is prefeced with ACK STX and postfixed with EOT +* \param[in] command the command to send +* \returns success of write and read of acknowledgement from controller +*/ +asynStatus SM300Controller::sendCommand(const char * command) { setTerminationChars("\x06", 1, "\x04", 1); //send data format 2 - sprintf(this->outString_, "\x06\x02%s", querry); + sprintf(this->outString_, "\x06\x02%s", command); return this->writeReadController(); } @@ -93,12 +96,17 @@ bool SM300Controller::is_moving() { return is_moving_; } +/** + * deal with db records being set which are integers + * return status + */ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { asynStatus status; int function = pasynUser->reason; //Function requested if (function == reset_) { if (value == 0) return asynSuccess; + setTerminationChars("\x04", 1, "\x04", 1); //send empty string with ACK to clear the buffer @@ -170,8 +178,8 @@ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { if (sendCommand("PYP0")) return status; // Feed for axes if (sendCommand("BF15000")) return status; - //TODO Do I need a PR? - + setIntegerParam(reset_, 0); + callParamCallbacks(); } else { status = asynMotorController::writeInt32(pasynUser, value); @@ -326,7 +334,8 @@ bool SM300Axis::has_error() { /** Polls the axis. * This function reads the position of the axis -* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). Not used as this is reported at the controller level +* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). +* \returns success of poll */ asynStatus SM300Axis::poll(bool *moving) { @@ -374,30 +383,13 @@ asynStatus SM300Axis::poll(bool *moving) void SM300Axis::report(FILE *fp, int level) { if (level > 0) { - fprintf(fp, " axis %d\n", - axisNo_ + 1); + fprintf(fp, " axis %d\n", axisNo_ + 1); } // Call the base class method asynMotorAxis::report(fp, level); } -asynStatus SM300Axis::sendAccelAndVelocity(double acceleration, double velocity) -{ - asynStatus status; - // static const char *functionName = "SM300::sendAccelAndVelocity"; - - // Send the velocity in egus - sprintf(pC_->outString_, "%1dVA%f", axisNo_ + 1, (velocity*stepSize_)); - status = pC_->writeController(); - - // Send the acceleration in egus/sec/sec - //printf("velocity: %f\n", velocity); - //printf("acceleration: %f", acceleration); - sprintf(pC_->outString_, "%1dAC%f", axisNo_ + 1, (acceleration*stepSize_)); - status = pC_->writeController(); - return status; -} /** Move the motor to an absolute location or by a relative amount (uses current location since the motor doesw not support a relative command). * \param[in] position The absolute position to move to (if relative=0) or the relative distance to move @@ -432,53 +424,14 @@ asynStatus SM300Axis::home(double minVelocity, double maxVelocity, double accele // Jog asynStatus SM300Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) { - double high_limit; - double low_limit; - asynStatus comStatus; - static const char *functionName = "SM300Axis::moveVelocity"; - - asynPrint(pasynUser_, ASYN_TRACE_FLOW, - "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n", - functionName, minVelocity, maxVelocity, acceleration); - - comStatus = sendAccelAndVelocity(acceleration, maxVelocity); - if (comStatus) goto skip; - - /* SM300 supports the notion of jog, but only for a remote control keypad */ - // SM300 will not allow moves outside of those set with the SL and SR commands - // first we query these limits and then make the jog a move to the limit - - // get the high limit - sprintf(pC_->outString_, "%1dSR?", axisNo_ + 1); - comStatus = pC_->writeReadController(); - if (comStatus) goto skip; - // The response string is of the form "1SR25.0" - high_limit = (atof(&pC_->inString_[3])); - - // get the low limit - sprintf(pC_->outString_, "%1dSL?", axisNo_ + 1); - comStatus = pC_->writeReadController(); - if (comStatus) goto skip; - // The response string is of the form "1SL-5.0" - low_limit = (atof(&pC_->inString_[3])); - - if (maxVelocity > 0.) { - /* This is a positive move in SM300 coordinates (egus) */ - sprintf(pC_->outString_, "%1dPA%f", axisNo_ + 1, high_limit); - } else { - /* This is a negative move in SM300 coordinates (egus) */ - sprintf(pC_->outString_, "%1dPA%f", axisNo_ + 1, low_limit); - } - comStatus = pC_->writeController(); - if (comStatus) goto skip; - - skip: - setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0); - callParamCallbacks(); - return comStatus ? asynError : asynSuccess; + errlogPrintf("Device does not support move velocity (jog)"); + return asynError; } +/** Stop of motor axis moving (Stops all axes) + * \returns send command status +*/ asynStatus SM300Axis::stop(double acceleration ) { asynStatus status; @@ -488,30 +441,15 @@ asynStatus SM300Axis::stop(double acceleration ) return status ? asynError : asynSuccess; } +/** Set absolute position in hardware + * Not supported + */ asynStatus SM300Axis::setPosition(double position) { - asynStatus status; - //static const char *functionName = "SM300Axis::setPosition"; - - // ? not sure yet - //sprintf(pC_->outString_, "#%02dP=%+d", axisNo_ + 1, NINT(position)); - status = pC_->writeReadController(); - return status; -} - -asynStatus SM300Axis::setClosedLoop(bool closedLoop) -{ - asynStatus status; - //static const char *functionName = "SM300Axis::setClosedLoop"; - - // ? not sure yet - //sprintf(pC_->outString_, "#%02dW=%d", axisNo_ + 1, closedLoop ? 1:0); - status = pC_->writeReadController(); - return status; + errlogPrintf("Device does not support set position"); + return asynError; } - - /** Code for iocsh registration */ static const iocshArg SM300CreateControllerArg0 = {"Port name", iocshArgString}; static const iocshArg SM300CreateControllerArg1 = {"SM300 port name", iocshArgString}; diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 9d6a4d501..4cf304aca 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -1,9 +1,5 @@ /* -FILENAME... SM300Driver.h -USAGE... Motor driver support for the SM300 controller. - -Based on the Newport SMC100 device driver written by: - +Motor driver support for the SM300 controller. */ @@ -23,12 +19,10 @@ class epicsShareClass SM300Axis : public asynMotorAxis asynStatus stop(double acceleration); asynStatus poll(bool *moving); asynStatus setPosition(double position); - asynStatus setClosedLoop(bool closedLoop); bool has_error(); private: SM300Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. * Abbreviated because it is used very frequently */ - asynStatus sendAccelAndVelocity(double accel, double velocity); double stepSize_; /**< Encoder increment value obtained with SU? command _or_ resolution, set at boot time */ /* with SMC100CreateController command */ char axisLabel; /** label for the axis*/ diff --git a/motorApp/SM300Src/SM300Register.h b/motorApp/SM300Src/SM300Register.h index 536c70a3f..3cbe329f0 100644 --- a/motorApp/SM300Src/SM300Register.h +++ b/motorApp/SM300Src/SM300Register.h @@ -1,6 +1,5 @@ /* -FILENAME... SM300Register.h -USAGE... This file contains function prototypes for SM300 IOC shell commands. +This file contains function prototypes for SM300 IOC shell commands. */ From 072a56510a70bb6068412db1761e928da5d1a8ec Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Fri, 5 Jan 2018 11:43:09 +0000 Subject: [PATCH 112/232] BCC ending form and send move after set sp --- motorApp/SM300Src/SM300Driver.cpp | 25 +++++++++++++++++++------ motorApp/SM300Src/SM300Driver.h | 2 +- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 161d279f9..e5919b17f 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -68,10 +68,16 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName * Querry string is prefeced with ACK STX and postfixed with EOT * return string ends with ETX * \param[in] querry the querry to send + * \param[in] what form the querry reply has; true if the reply has EOT false for ETX with BCC * \returns success of write and read of querry string */ -asynStatus SM300Controller::sendQuerry(const char * querry) { - setTerminationChars("\x04", 1, "\x04", 1); +asynStatus SM300Controller::sendQuerry(const char * querry, bool hasEotEnding) { + if (hasEotEnding) { + setTerminationChars("\x04", 1, "\x04", 1); + } + else { + setTerminationChars("\x03", 1, "\x04", 1); + } //send data format 2 sprintf(this->outString_, "\x06\x02%s", querry); return this->writeReadController(); @@ -267,7 +273,7 @@ asynStatus SM300Controller::poll() SM300Axis *axis; // Read the current status of the motor (at Poition, Not at position, Error) - comStatus = this->sendQuerry("LM"); + comStatus = this->sendQuerry("LM", true); if (comStatus) goto skip; if (strlen(this->inString_) < 3) { @@ -347,7 +353,7 @@ asynStatus SM300Axis::poll(bool *moving) // Read the current motor position sprintf(temp, "LI%c", this->axisLabel); - comStatus = pC_->sendQuerry(temp); + comStatus = pC_->sendQuerry(temp, false); if (comStatus) goto skip; // The response string is of the form "\06\02%d" @@ -400,6 +406,7 @@ void SM300Axis::report(FILE *fp, int level) * \param[in] acceleration The acceleration value. Units=steps/sec/sec. */ asynStatus SM300Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { + asynStatus comStatus; char temp[MAX_CONTROLLER_STRING_SIZE]; double move_to = position; @@ -411,7 +418,13 @@ asynStatus SM300Axis::move(double position, int relative, double minVelocity, do } sprintf(temp, "B%c%.0f", axisLabel, round(move_to)); - return pC_->sendCommand(temp); + comStatus = pC_->sendCommand(temp); + if (comStatus) goto skip; + comStatus = pC_->sendCommand("BSL"); + +skip: + return comStatus; + } asynStatus SM300Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) @@ -436,7 +449,7 @@ asynStatus SM300Axis::stop(double acceleration ) { asynStatus status; - status = pC_->sendQuerry("BSS"); + status = pC_->sendQuerry("BSS", false); // ignore reply it has to be P return status ? asynError : asynSuccess; } diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 4cf304aca..f8c8a0876 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -42,7 +42,7 @@ class SM300Controller : public asynMotorController { virtual asynStatus poll(); asynStatus sendCommand(const char * querry); - asynStatus sendQuerry(const char * querry); + asynStatus sendQuerry(const char * querry, bool hasEotEnding); bool has_error(); bool is_moving(); From 236904d2490783f90c477a66e85b527901ec76d7 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Fri, 5 Jan 2018 16:46:52 +0000 Subject: [PATCH 113/232] Send stop before setting position to enable position on different axis --- motorApp/SM300Src/SM300Driver.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index e5919b17f..dbbeaece1 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -416,9 +416,14 @@ asynStatus SM300Axis::move(double position, int relative, double minVelocity, do errlogPrintf("SM300 move: Can not do relative move with this motor.\n"); return asynError; } - + + // perform a stop so new positions can be set + comStatus = pC_->sendQuerry("BSS", false); + if (comStatus) goto skip; + sprintf(temp, "B%c%.0f", axisLabel, round(move_to)); comStatus = pC_->sendCommand(temp); + if (comStatus) goto skip; comStatus = pC_->sendCommand("BSL"); From 04e44c2ca7538129653bef695f17ce691f01d5c0 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Mon, 8 Jan 2018 11:25:56 +0000 Subject: [PATCH 114/232] Remove step size per controller --- motorApp/SM300Src/SM300Driver.cpp | 24 +++++++----------------- motorApp/SM300Src/SM300Driver.h | 2 +- 2 files changed, 8 insertions(+), 18 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index dbbeaece1..85adebc2e 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -33,7 +33,7 @@ Based on the SM100 Model 3 device driver * \param[in] idlePollPeriod The time between polls when no axis is moving */ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName, int numAxes, - double movingPollPeriod, double idlePollPeriod, double stepSize) + double movingPollPeriod, double idlePollPeriod) : is_moving_(false), asynMotorController(portName, numAxes, NUM_SM300_PARAMS, 0, // No additional interfaces beyond those in base class @@ -119,10 +119,9 @@ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { status = this->writeController(); //set termination character is EOT without check sum * - // when sedning send incase this mode is switched on + // when sending send incase this mode is switched on setTerminationChars("\x06", 1, "\x04\x0D", 2); if (sendCommand("PEK0")) return status; - setTerminationChars("\x06", 1, "\x04", 1); // set achknowlegement on @@ -204,17 +203,10 @@ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { * \param[in] eguPerStep The stage resolution */ extern "C" int SM300CreateController(const char *portName, const char *SM300PortName, int numAxes, - int movingPollPeriod, int idlePollPeriod, const char *eguPerStep) + int movingPollPeriod, int idlePollPeriod) { - double stepSize; - - stepSize = strtod(eguPerStep, NULL); - new SM300Controller(portName, SM300PortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000., stepSize); + new SM300Controller(portName, SM300PortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.); //printf("\n *** SM300: stepSize=%f\n", stepSize); - if (errno != 0) { - printf("SM300: Error invalid steps per unit=%s\n", eguPerStep); - return asynError; - } return(asynSuccess); } @@ -474,17 +466,15 @@ static const iocshArg SM300CreateControllerArg1 = {"SM300 port name", iocshArgSt static const iocshArg SM300CreateControllerArg2 = {"Number of axes", iocshArgInt}; static const iocshArg SM300CreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt}; static const iocshArg SM300CreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt}; -static const iocshArg SM300CreateControllerArg5 = {"EGUs per step", iocshArgString}; static const iocshArg * const SM300CreateControllerArgs[] = {&SM300CreateControllerArg0, &SM300CreateControllerArg1, &SM300CreateControllerArg2, &SM300CreateControllerArg3, - &SM300CreateControllerArg4, - &SM300CreateControllerArg5}; -static const iocshFuncDef SM300CreateControllerDef = {"SM300CreateController", 6, SM300CreateControllerArgs}; + &SM300CreateControllerArg4}; +static const iocshFuncDef SM300CreateControllerDef = {"SM300CreateController", 5, SM300CreateControllerArgs}; static void SM300CreateContollerCallFunc(const iocshArgBuf *args) { - SM300CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].sval); + SM300CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival); } static void SM300Register(void) diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index f8c8a0876..0ac1c8626 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -32,7 +32,7 @@ friend class SM300Controller; class SM300Controller : public asynMotorController { public: - SM300Controller(const char *portName, const char *SMC100PortName, int numAxes, double movingPollPeriod, double idlePollPeriod, double stepSize); + SM300Controller(const char *portName, const char *SMC100PortName, int numAxes, double movingPollPeriod, double idlePollPeriod); void report(FILE *fp, int level); SM300Axis* getAxis(asynUser *pasynUser); SM300Axis* getAxis(int axisNo); From eecf8024300198479a653641141529cd2c61342b Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Mon, 8 Jan 2018 12:12:31 +0000 Subject: [PATCH 115/232] Add disconnect --- motorApp/Db/SM300_extra.db | 12 +++++++++++- motorApp/SM300Src/SM300Driver.cpp | 9 +++++++++ motorApp/SM300Src/SM300Driver.h | 4 +++- 3 files changed, 23 insertions(+), 2 deletions(-) diff --git a/motorApp/Db/SM300_extra.db b/motorApp/Db/SM300_extra.db index 0fd19e56c..0892c9833 100644 --- a/motorApp/Db/SM300_extra.db +++ b/motorApp/Db/SM300_extra.db @@ -1,7 +1,7 @@ # Database for SM300 record(busy,"$(P)RESET") { -field(DTYP, "asynInt32") + field(DTYP, "asynInt32") field(DESC,"Reset the motor parameters") field(ZNAM, "Done") @@ -9,3 +9,13 @@ field(DTYP, "asynInt32") field(VAL, "0") field(OUT, "@asyn($(PORT))RESET") } + +record(busy, "$(P)DISCONNECT") { + field(DTYP, "asynInt32") + field(DESC,"Disconnect controller from IOC") + + field(ZNAM, "Done") + field(ONAM, "Disconnecting") + field(VAL, "0") + field(OUT, "@asyn($(PORT))DISCONNECT") +} diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 85adebc2e..8b4a02483 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -61,6 +61,8 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName createParam(SM300ResetString, asynParamInt32, &reset_); + createParam(SM300DisconnectString, asynParamInt32, &disconnect_); + startPoller(movingPollPeriod, idlePollPeriod, 2); } @@ -186,6 +188,13 @@ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { setIntegerParam(reset_, 0); callParamCallbacks(); } + else if (function == disconnect_) { + setTerminationChars("\x06", 1, "\x04", 1); + sprintf(this->outString_, "\x06\x02%s", "M77"); + status = writeController(); + setIntegerParam(disconnect_, 0); + callParamCallbacks(); + } else { status = asynMotorController::writeInt32(pasynUser, value); } diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 0ac1c8626..8feadefe5 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -4,8 +4,9 @@ Motor driver support for the SM300 controller. // controller-specific parameters yet -#define NUM_SM300_PARAMS 1 +#define NUM_SM300_PARAMS 2 #define SM300ResetString "RESET" +#define SM300DisconnectString "DISCONNECT" class epicsShareClass SM300Axis : public asynMotorAxis { @@ -50,6 +51,7 @@ class SM300Controller : public asynMotorController { bool has_error_; bool is_moving_; int reset_; + int disconnect_; friend class SM300Axis; From 3825c43b02c608a1fe7338f6274d3037565628ca Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Mon, 8 Jan 2018 15:37:24 +0000 Subject: [PATCH 116/232] Add read error status --- motorApp/Db/SM300_extra.db | 74 +++++++++++++++++++++++++++++++ motorApp/SM300Src/SM300Driver.cpp | 26 ++++++++++- motorApp/SM300Src/SM300Driver.h | 4 +- 3 files changed, 101 insertions(+), 3 deletions(-) diff --git a/motorApp/Db/SM300_extra.db b/motorApp/Db/SM300_extra.db index 0892c9833..dab4b9de9 100644 --- a/motorApp/Db/SM300_extra.db +++ b/motorApp/Db/SM300_extra.db @@ -19,3 +19,77 @@ record(busy, "$(P)DISCONNECT") { field(VAL, "0") field(OUT, "@asyn($(PORT))DISCONNECT") } + +record(mbbi, "$(P)ERROR") { + + field(DTYP, "asynInt32") + field(DESC, "Error code") + field(SCAN, "1 second") + + field(ZRST, "") + field(ZRSV, "NO_ALARM") + field(ZRVL, "0") + + field(ONST, "Servo error") + field(ONSV, "MAJOR") + field(ONVL, "1") + + field(TWST, "Standstill check") + field(TWSV, "MAJOR") + field(TWVL, "2") + + field(THST, "Drag error") + field(THSV, "MAJOR") + field(THVL, "3") + + field(FRST, "Software limit switch") + field(FRSV, "MAJOR") + field(FRVL, "4") + + field(FVST, "Hardware limit switch") + field(FVSV, "MAJOR") + field(FVVL, "5") + + field(SXST, "DAC overflow") + field(SXSV, "MAJOR") + field(SXVL, "6") + + field(SVST, "Unknown") + field(SVSV, "MAJOR") + field(SVVL, "7") + + field(EIST, "Unknown") + field(EISV, "MAJOR") + field(EIVL, "8") + + field(NIST, "Unknown") + field(NISV, "MAJOR") + field(NIVL, "9") + + field(TEST, "No RAM memory card exists") + field(TESV, "MAJOR") + field(TEVL, "10") + + field(ELST, "RAM card not formatted") + field(ELSV, "MAJOR") + field(ELVL, "11") + + field(TWST, "Unknown") + field(TWSV, "MAJOR") + field(TWVL, "12") + + field(TTST, "Cmd rep x4 no success") + field(TTSV, "MAJOR") + field(TTVL, "13") + + field(FTST, "CNC cmd error code") + field(FTSV, "MAJOR") + field(FTVL, "14") + + field(FFST, "Cmd error code") + field(FFSV, "MAJOR") + field(FFVL, "15") + + field(VAL, "0") + field(INP, "@asyn($(PORT))ERROR_CODE") +} diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 8b4a02483..1062882ef 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -62,6 +62,7 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName createParam(SM300ResetString, asynParamInt32, &reset_); createParam(SM300DisconnectString, asynParamInt32, &disconnect_); + createParam(SM300ErrorCodeString, asynParamInt32, &error_code_); startPoller(movingPollPeriod, idlePollPeriod, 2); } @@ -269,7 +270,6 @@ void SM300Controller::setTerminationChars(const char *eosIn, int eosInlen, const */ asynStatus SM300Controller::poll() { - asynStatus comStatus; SM300Axis *axis; @@ -299,6 +299,28 @@ asynStatus SM300Controller::poll() axis = this->getAxis(i); axis->setIntegerParam(motorStatusDone_, is_moving_ ? 0 : 1); } + + comStatus = sendQuerry("LS10", false); + if (comStatus) goto skip; + + if (strlen(this->inString_) < 3) { + comStatus = asynError; + errlogPrintf("SM300 error code: return string is too short.\n"); + goto skip; + } + int code = strtol(&this->inString_[2], NULL, 16); + // special case for error in sending command + if (code >= 0xF && code <= 0x14) { + errlogPrintf("SM300 error code: CNC cmd error code, code = %.2x\n", code); + code = 0xF; + } + // speacial case for error in CNC interpreter + if (code >= 0x1E && code <= 0x4F) { + errlogPrintf("SM300 error code: CNC cmd error, code = %.2x\n", code); + code = 0xE; + } + + setIntegerParam(error_code_, code); skip: has_error_ = comStatus != asynSuccess; @@ -307,7 +329,7 @@ asynStatus SM300Controller::poll() axis = this->getAxis(i); axis->setIntegerParam(this->motorStatusProblem_, axis->has_error() || has_error_ ? 1 : 0); axis->callParamCallbacks(); - } + } callParamCallbacks(); return comStatus ? asynError : asynSuccess; } diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 8feadefe5..a0c817611 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -4,9 +4,10 @@ Motor driver support for the SM300 controller. // controller-specific parameters yet -#define NUM_SM300_PARAMS 2 +#define NUM_SM300_PARAMS 3 #define SM300ResetString "RESET" #define SM300DisconnectString "DISCONNECT" +#define SM300ErrorCodeString "ERROR_CODE" class epicsShareClass SM300Axis : public asynMotorAxis { @@ -52,6 +53,7 @@ class SM300Controller : public asynMotorController { bool is_moving_; int reset_; int disconnect_; + int error_code_; friend class SM300Axis; From e9484e32c367addd3df250c87748583b994d7382 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 16 Jan 2018 15:47:03 +0000 Subject: [PATCH 117/232] Dont stop and home one axis at a time --- motorApp/Db/SM300_extra.db | 15 +- motorApp/SM300Src/SM300Driver.cpp | 247 +++++++++++++++++++----------- motorApp/SM300Src/SM300Driver.h | 13 +- 3 files changed, 181 insertions(+), 94 deletions(-) diff --git a/motorApp/Db/SM300_extra.db b/motorApp/Db/SM300_extra.db index dab4b9de9..73f2c13f4 100644 --- a/motorApp/Db/SM300_extra.db +++ b/motorApp/Db/SM300_extra.db @@ -1,9 +1,22 @@ # Database for SM300 +record (busy, "$(P)RESET_AND_HOME") +{ + field(DTYP, "asynInt32") + field(DESC,"Reset and home the motor") + field(SCAN, "I/O Intr") + + field(ZNAM, "Done") + field(ONAM, "Reseting") + field(VAL, "0") + field(OUT, "@asyn($(PORT))RESET_AND_HOME") +} + + record(busy,"$(P)RESET") { field(DTYP, "asynInt32") field(DESC,"Reset the motor parameters") - + field(SCAN, "I/O Intr") field(ZNAM, "Done") field(ONAM, "Reseting") field(VAL, "0") diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 1062882ef..0be942ffa 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -59,8 +59,8 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName pAxis = new SM300Axis(this, 0, 'X'); pAxis = new SM300Axis(this, 1, 'Y'); - createParam(SM300ResetString, asynParamInt32, &reset_); + createParam(SM300ResetAndHomeString, asynParamInt32, &reset_and_home_); createParam(SM300DisconnectString, asynParamInt32, &disconnect_); createParam(SM300ErrorCodeString, asynParamInt32, &error_code_); @@ -107,7 +107,7 @@ bool SM300Controller::is_moving() { /** * deal with db records being set which are integers - * return status + * \returns status */ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { asynStatus status; @@ -115,93 +115,114 @@ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { int function = pasynUser->reason; //Function requested if (function == reset_) { if (value == 0) return asynSuccess; - - setTerminationChars("\x04", 1, "\x04", 1); - - //send empty string with ACK to clear the buffer - status = this->writeController(); - - //set termination character is EOT without check sum * - // when sending send incase this mode is switched on - setTerminationChars("\x06", 1, "\x04\x0D", 2); - if (sendCommand("PEK0")) return status; - - setTerminationChars("\x06", 1, "\x04", 1); - // set achknowlegement on - if (sendCommand("PEL1")) return status; - // set linear interpolation - if (sendCommand("B/ G01")) return status; - // absoulute coordinates - if (sendCommand("B/ G90")) return status; - // switch off message from contrl unit when motor is in position or in error (will poll instead) - if (sendCommand("PER0")) return status; - - //send data format 2 - 100s - if (sendCommand("PXA2")) return status; - if (sendCommand("PYA2")) return status; - // Gear factor numerator - if (sendCommand("PXB5")) return status; - if (sendCommand("PYB1")) return status; - // Gear factor denomenator - if (sendCommand("PXC10")) return status; - if (sendCommand("PYC10")) return status; - // Drag Error - if (sendCommand("PXD2500")) return status; - if (sendCommand("PYD2500")) return status; - // Start/stop ramp - if (sendCommand("PXE100000")) return status; - if (sendCommand("PYE25000")) return status; - // KV factor oe feedback control amplification - if (sendCommand("PXF1000")) return status; - if (sendCommand("PYF1000")) return status; - // Regulator factor A - if (sendCommand("PXG0")) return status; - if (sendCommand("PYG0")) return status; - // +Software switch limit - if (sendCommand("PXH+57000")) return status; - if (sendCommand("PYH+64000")) return status; - // -Software switch limit - if (sendCommand("PXI-50")) return status; - if (sendCommand("PYI-20")) return status; - // maximum speed - if (sendCommand("PXJ25000")) return status; - if (sendCommand("PYJ25000")) return status; - // direction and speed of homing procedure - if (sendCommand("PXK-2500")) return status; - if (sendCommand("PYK-7500")) return status; - // standstill check - if (sendCommand("PXL100")) return status; - if (sendCommand("PYL100")) return status; - // Inposition window - if (sendCommand("PXM5")) return status; - if (sendCommand("PYM5")) return status; - // Distance from reference switch - if (sendCommand("PXN1000")) return status; - if (sendCommand("PYN5000")) return status; - // Backlash compensation - if (sendCommand("PXO0")) return status; - if (sendCommand("PYO0")) return status; - // Moving direction - if (sendCommand("PXP0")) return status; - if (sendCommand("PYP0")) return status; - // Feed for axes - if (sendCommand("BF15000")) return status; - setIntegerParam(reset_, 0); + status = perform_reset(); callParamCallbacks(); } else if (function == disconnect_) { + setIntegerParam(disconnect_, 1); setTerminationChars("\x06", 1, "\x04", 1); sprintf(this->outString_, "\x06\x02%s", "M77"); status = writeController(); setIntegerParam(disconnect_, 0); callParamCallbacks(); } + else if (function == reset_and_home_) { + setIntegerParam(reset_and_home_, 1); + callParamCallbacks(); + perform_reset(); + for (int i = 0; i < numAxes_; i++) { + getAxis(i)->home(0, 0, 0, 0); + } + setIntegerParam(reset_and_home_, 0); + callParamCallbacks(); + } else { status = asynMotorController::writeInt32(pasynUser, value); } return status; } +/** + * Perform a reset of the controller sending it initiation parameters + * \returns status +*/ +asynStatus SM300Controller::perform_reset() { + setIntegerParam(reset_, 1); + callParamCallbacks(); + setTerminationChars("\x04", 1, "\x04", 1); + + //send empty string with ACK to clear the buffer + asynStatus status = this->writeController(); + + //set termination character is EOT without check sum * + // when sending send incase this mode is switched on + setTerminationChars("\x06", 1, "\x04\x0D", 2); + if (sendCommand("PEK0")) return status; + + setTerminationChars("\x06", 1, "\x04", 1); + // set achknowlegement on + if (sendCommand("PEL1")) return status; + // set linear interpolation + if (sendCommand("B/ G01")) return status; + // absoulute coordinates + if (sendCommand("B/ G90")) return status; + // switch off message from contrl unit when motor is in position or in error (will poll instead) + if (sendCommand("PER0")) return status; + + //send data format 2 - 100s + if (sendCommand("PXA2")) return status; + if (sendCommand("PYA2")) return status; + // Gear factor numerator + if (sendCommand("PXB5")) return status; + if (sendCommand("PYB1")) return status; + // Gear factor denomenator + if (sendCommand("PXC10")) return status; + if (sendCommand("PYC10")) return status; + // Drag Error + if (sendCommand("PXD2500")) return status; + if (sendCommand("PYD2500")) return status; + // Start/stop ramp + if (sendCommand("PXE100000")) return status; + if (sendCommand("PYE25000")) return status; + // KV factor oe feedback control amplification + if (sendCommand("PXF1000")) return status; + if (sendCommand("PYF1000")) return status; + // Regulator factor A + if (sendCommand("PXG0")) return status; + if (sendCommand("PYG0")) return status; + // +Software switch limit + if (sendCommand("PXH+57000")) return status; + if (sendCommand("PYH+64000")) return status; + // -Software switch limit + if (sendCommand("PXI-50")) return status; + if (sendCommand("PYI-20")) return status; + // maximum speed + if (sendCommand("PXJ25000")) return status; + if (sendCommand("PYJ25000")) return status; + // direction and speed of homing procedure + if (sendCommand("PXK-2500")) return status; + if (sendCommand("PYK-7500")) return status; + // standstill check + if (sendCommand("PXL100")) return status; + if (sendCommand("PYL100")) return status; + // Inposition window + if (sendCommand("PXM5")) return status; + if (sendCommand("PYM5")) return status; + // Distance from reference switch + if (sendCommand("PXN1000")) return status; + if (sendCommand("PYN5000")) return status; + // Backlash compensation + if (sendCommand("PXO0")) return status; + if (sendCommand("PYO0")) return status; + // Moving direction + if (sendCommand("PXP0")) return status; + if (sendCommand("PYP0")) return status; + // Feed for axes + if (sendCommand("BF15000")) return status; + setIntegerParam(reset_, 0); + return asynSuccess; +} + /** Creates a new SM300Controller object. * Configuration command, called directly or from iocsh @@ -272,6 +293,7 @@ asynStatus SM300Controller::poll() { asynStatus comStatus; SM300Axis *axis; + bool motorInError = false; // Read the current status of the motor (at Poition, Not at position, Error) comStatus = this->sendQuerry("LM", true); @@ -283,17 +305,41 @@ asynStatus SM300Controller::poll() goto skip; } - if (this->inString_[2] == 'P') { + if (this->inString_[2] == 'P') { // axis in Position is_moving_ = false; + + // Finished a home + if (axis_x_homing_) { + axis_x_homing_ = false; + home_axis_x_ = false; + } + if (axis_y_homing_) { + axis_y_homing_ = false; + home_axis_y_ = false; + } + // If need to home to now + if (home_axis_x_) { + comStatus = sendCommand("BRX"); + axis_x_homing_ = true; + if (comStatus) { + errlogPrintf("SM300 poll: Failed to home x.\n"); + } + } else if (home_axis_y_) { + comStatus = sendCommand("BRY"); + axis_y_homing_ = true; + if (comStatus) { + errlogPrintf("SM300 poll: Failed to home y.\n"); + } + } } - else if (this->inString_[2] == 'N') { + else if (this->inString_[2] == 'N') { // Not in position is_moving_ = true; } else { + motorInError = true; is_moving_ = false; comStatus = asynError; - errlogPrintf("SM300 poll: moving status returned error status.\n"); - goto skip; + errlogPrintf("SM300 poll: moving status returned errors status.\n"); } for (int i=0; i < this->numAxes_; i++) { axis = this->getAxis(i); @@ -309,6 +355,11 @@ asynStatus SM300Controller::poll() goto skip; } int code = strtol(&this->inString_[2], NULL, 16); + + if (code != 0) { + motorInError = true; + } + // special case for error in sending command if (code >= 0xF && code <= 0x14) { errlogPrintf("SM300 error code: CNC cmd error code, code = %.2x\n", code); @@ -323,7 +374,7 @@ asynStatus SM300Controller::poll() setIntegerParam(error_code_, code); skip: - has_error_ = comStatus != asynSuccess; + has_error_ = motorInError || comStatus != asynSuccess; for (int i=0; i < this->numAxes_; i++) { axis = this->getAxis(i); @@ -440,10 +491,6 @@ asynStatus SM300Axis::move(double position, int relative, double minVelocity, do return asynError; } - // perform a stop so new positions can be set - comStatus = pC_->sendQuerry("BSS", false); - if (comStatus) goto skip; - sprintf(temp, "B%c%.0f", axisLabel, round(move_to)); comStatus = pC_->sendCommand(temp); @@ -455,11 +502,32 @@ asynStatus SM300Axis::move(double position, int relative, double minVelocity, do } +/** Home an axis + * + * This stores the intention to home when the motor comes to a stop. The reason it is done this way is that if the motors is homing + * one axis it can not home any other axis. It wil home the next time the motor is stationary. + * /param[in] axis the axis to home (X or Y) +*/ +void SM300Controller::homeAxis(const char axis) { + if (axis == 'X') { + if (!axis_x_homing_) { + home_axis_x_ = true; + } + } + if (axis == 'Y') { + if (!axis_y_homing_) { + home_axis_y_ = true; + } + } +} + +/** Home the axis + * velocities and accelerations are ignored these are set by the controller. + */ asynStatus SM300Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) { - char temp[MAX_CONTROLLER_STRING_SIZE]; - sprintf(temp, "BR%c", axisLabel); - return pC_->sendCommand(temp); + pC_->homeAxis(axisLabel); + return asynSuccess; } // Jog @@ -475,11 +543,8 @@ asynStatus SM300Axis::moveVelocity(double minVelocity, double maxVelocity, doubl */ asynStatus SM300Axis::stop(double acceleration ) { - asynStatus status; - - status = pC_->sendQuerry("BSS", false); - // ignore reply it has to be P - return status ? asynError : asynSuccess; + // Motor can not be stopped using BSS because if it is then it immediately sets the position to 0 wherever the motor stops. + return asynSuccess; } /** Set absolute position in hardware diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index a0c817611..e335c16d7 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -4,8 +4,9 @@ Motor driver support for the SM300 controller. // controller-specific parameters yet -#define NUM_SM300_PARAMS 3 +#define NUM_SM300_PARAMS 4 #define SM300ResetString "RESET" +#define SM300ResetAndHomeString "RESET_AND_HOME" #define SM300DisconnectString "DISCONNECT" #define SM300ErrorCodeString "ERROR_CODE" @@ -47,13 +48,21 @@ class SM300Controller : public asynMotorController { asynStatus sendQuerry(const char * querry, bool hasEotEnding); bool has_error(); bool is_moving(); - + void homeAxis(const char axis); private: + asynStatus perform_reset(); + bool has_error_; bool is_moving_; int reset_; + int reset_and_home_; int disconnect_; int error_code_; + bool axis_x_homing_; + bool axis_y_homing_; + bool home_axis_x_; + bool home_axis_y_; + friend class SM300Axis; From d49727563f2ca8b9177f3f1fb2734f979e4e5885 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 23 Jan 2018 16:29:35 +0000 Subject: [PATCH 118/232] style changes --- motorApp/SM300Src/SM300Driver.cpp | 147 +++++++++++++++--------------- motorApp/SM300Src/SM300Driver.h | 21 +++-- 2 files changed, 90 insertions(+), 78 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 0be942ffa..fd6c69c2f 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -23,8 +23,6 @@ Based on the SM100 Model 3 device driver #include #include "SM300Driver.h" -#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5) - /** Creates a new SM300Controller object. * \param[in] portName The name of the asyn port that will be created for this driver * \param[in] SM300PortName The name of the drvAsynSerialPort that was created previously to connect to the SM300 controller @@ -67,14 +65,14 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName startPoller(movingPollPeriod, idlePollPeriod, 2); } -/** Send a querry string to the controller and get a return value. - * Querry string is prefeced with ACK STX and postfixed with EOT +/** Send a query string to the controller and get a return value. + * query string is prefeced with ACK STX and postfixed with EOT * return string ends with ETX - * \param[in] querry the querry to send - * \param[in] what form the querry reply has; true if the reply has EOT false for ETX with BCC - * \returns success of write and read of querry string + * \param[in] query the query to send + * \param[in] what form the query reply has; true if the reply has EOT false for ETX with BCC + * \returns success of write and read of query string */ -asynStatus SM300Controller::sendQuerry(const char * querry, bool hasEotEnding) { +asynStatus SM300Controller::sendQuery(const char * query, bool hasEotEnding) { if (hasEotEnding) { setTerminationChars("\x04", 1, "\x04", 1); } @@ -82,7 +80,7 @@ asynStatus SM300Controller::sendQuerry(const char * querry, bool hasEotEnding) { setTerminationChars("\x03", 1, "\x04", 1); } //send data format 2 - sprintf(this->outString_, "\x06\x02%s", querry); + sprintf(this->outString_, "\x06\x02%s", query); return this->writeReadController(); } @@ -160,65 +158,72 @@ asynStatus SM300Controller::perform_reset() { if (sendCommand("PEK0")) return status; setTerminationChars("\x06", 1, "\x04", 1); - // set achknowlegement on - if (sendCommand("PEL1")) return status; - // set linear interpolation - if (sendCommand("B/ G01")) return status; - // absoulute coordinates - if (sendCommand("B/ G90")) return status; - // switch off message from contrl unit when motor is in position or in error (will poll instead) - if (sendCommand("PER0")) return status; - - //send data format 2 - 100s - if (sendCommand("PXA2")) return status; - if (sendCommand("PYA2")) return status; - // Gear factor numerator - if (sendCommand("PXB5")) return status; - if (sendCommand("PYB1")) return status; - // Gear factor denomenator - if (sendCommand("PXC10")) return status; - if (sendCommand("PYC10")) return status; - // Drag Error - if (sendCommand("PXD2500")) return status; - if (sendCommand("PYD2500")) return status; - // Start/stop ramp - if (sendCommand("PXE100000")) return status; - if (sendCommand("PYE25000")) return status; - // KV factor oe feedback control amplification - if (sendCommand("PXF1000")) return status; - if (sendCommand("PYF1000")) return status; - // Regulator factor A - if (sendCommand("PXG0")) return status; - if (sendCommand("PYG0")) return status; - // +Software switch limit - if (sendCommand("PXH+57000")) return status; - if (sendCommand("PYH+64000")) return status; - // -Software switch limit - if (sendCommand("PXI-50")) return status; - if (sendCommand("PYI-20")) return status; - // maximum speed - if (sendCommand("PXJ25000")) return status; - if (sendCommand("PYJ25000")) return status; - // direction and speed of homing procedure - if (sendCommand("PXK-2500")) return status; - if (sendCommand("PYK-7500")) return status; - // standstill check - if (sendCommand("PXL100")) return status; - if (sendCommand("PYL100")) return status; - // Inposition window - if (sendCommand("PXM5")) return status; - if (sendCommand("PYM5")) return status; - // Distance from reference switch - if (sendCommand("PXN1000")) return status; - if (sendCommand("PYN5000")) return status; - // Backlash compensation - if (sendCommand("PXO0")) return status; - if (sendCommand("PYO0")) return status; - // Moving direction - if (sendCommand("PXP0")) return status; - if (sendCommand("PYP0")) return status; - // Feed for axes - if (sendCommand("BF15000")) return status; + const char * commands[] = { + // set achknowlegement on + "PEL1", + // set linear interpolation + "B/ G01", + // absoulute coordinates + "B/ G90", + // switch off message from contrl unit when motor is in position or in error (will poll instead) + "PER0", + //send data format 2 - 100s + "PXA2", + "PYA2", + // Gear factor numerator + "PXB5", + "PYB1", + // Gear factor denomenator + "PXC10", + "PYC10", + // Drag Error + "PXD2500", + "PYD2500", + // Start/stop ramp + "PXE100000", + "PYE25000", + // KV factor oe feedback control amplification + "PXF1000", + "PYF1000", + // Regulator factor A + "PXG0", + "PYG0", + // +Software switch limit + "PXH+57000", + "PYH+64000", + // -Software switch limit + "PXI-50", + "PYI-20", + // maximum speed + "PXJ25000", + "PYJ25000", + // direction and speed of homing procedure + "PXK-2500", + "PYK-7500", + // standstill check + "PXL100", + "PYL100", + // Inposition window + "PXM5", + "PYM5", + // Distance from reference switch + "PXN1000", + "PYN5000", + // Backlash compensation + "PXO0", + "PYO0", + // Moving direction + "PXP0", + "PYP0", + // Feed for axes + "BF15000" + }; + + int n_array = (sizeof(commands) / sizeof(const char *)); + for (int i = 0; i < n_array; i++) { + if (sendCommand(commands[i])) return status; + } + setIntegerParam(reset_, 0); return asynSuccess; } @@ -296,7 +301,7 @@ asynStatus SM300Controller::poll() bool motorInError = false; // Read the current status of the motor (at Poition, Not at position, Error) - comStatus = this->sendQuerry("LM", true); + comStatus = this->sendQuery("LM", true); if (comStatus) goto skip; if (strlen(this->inString_) < 3) { @@ -346,7 +351,7 @@ asynStatus SM300Controller::poll() axis->setIntegerParam(motorStatusDone_, is_moving_ ? 0 : 1); } - comStatus = sendQuerry("LS10", false); + comStatus = sendQuery("LS10", false); if (comStatus) goto skip; if (strlen(this->inString_) < 3) { @@ -427,7 +432,7 @@ asynStatus SM300Axis::poll(bool *moving) // Read the current motor position sprintf(temp, "LI%c", this->axisLabel); - comStatus = pC_->sendQuerry(temp, false); + comStatus = pC_->sendQuery(temp, false); if (comStatus) goto skip; // The response string is of the form "\06\02%d" diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index e335c16d7..716e722b3 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -2,9 +2,10 @@ Motor driver support for the SM300 controller. */ +#ifndef SM300Driver_H +#define SM300Driver_H // controller-specific parameters yet -#define NUM_SM300_PARAMS 4 #define SM300ResetString "RESET" #define SM300ResetAndHomeString "RESET_AND_HOME" #define SM300DisconnectString "DISCONNECT" @@ -44,8 +45,8 @@ class SM300Controller : public asynMotorController { virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); virtual asynStatus poll(); - asynStatus sendCommand(const char * querry); - asynStatus sendQuerry(const char * querry, bool hasEotEnding); + asynStatus sendCommand(const char * query); + asynStatus sendQuery(const char * query, bool hasEotEnding); bool has_error(); bool is_moving(); void homeAxis(const char axis); @@ -54,16 +55,22 @@ class SM300Controller : public asynMotorController { bool has_error_; bool is_moving_; - int reset_; - int reset_and_home_; - int disconnect_; - int error_code_; bool axis_x_homing_; bool axis_y_homing_; bool home_axis_x_; bool home_axis_y_; + #define FIRST_SM300_PARAM reset_ + int reset_; + int reset_and_home_; + int disconnect_; + int error_code_; + #define LAST_SM300_PARAM error_code_ + friend class SM300Axis; }; + +#define NUM_SM300_PARAMS (&LAST_SM300_PARAM - &FIRST_SM300_PARAM + 1) +#endif // SM300Driver_H From 1466b5b7fb6646d8c16936bd9dc01f522c29c18e Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Fri, 26 Jan 2018 15:36:19 +0000 Subject: [PATCH 119/232] Change from AMJOR ot Comm error for communcation errors. Make sure error is set on start --- motorApp/SM300Src/SM300Driver.cpp | 27 ++++++++++++++++++--------- motorApp/SM300Src/SM300Driver.h | 5 +++++ 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index fd6c69c2f..478047c6a 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -33,6 +33,7 @@ Based on the SM100 Model 3 device driver SM300Controller::SM300Controller(const char *portName, const char *SM300PortName, int numAxes, double movingPollPeriod, double idlePollPeriod) : is_moving_(false), + polls_count_(0), asynMotorController(portName, numAxes, NUM_SM300_PARAMS, 0, // No additional interfaces beyond those in base class 0, // No additional callback interfaces beyond those in base class @@ -298,7 +299,7 @@ asynStatus SM300Controller::poll() { asynStatus comStatus; SM300Axis *axis; - bool motorInError = false; + bool motorHasError = false; // Read the current status of the motor (at Poition, Not at position, Error) comStatus = this->sendQuery("LM", true); @@ -341,11 +342,12 @@ asynStatus SM300Controller::poll() is_moving_ = true; } else { - motorInError = true; + motorHasError = true; is_moving_ = false; comStatus = asynError; errlogPrintf("SM300 poll: moving status returned errors status.\n"); } + for (int i=0; i < this->numAxes_; i++) { axis = this->getAxis(i); axis->setIntegerParam(motorStatusDone_, is_moving_ ? 0 : 1); @@ -362,7 +364,7 @@ asynStatus SM300Controller::poll() int code = strtol(&this->inString_[2], NULL, 16); if (code != 0) { - motorInError = true; + motorHasError = true; } // special case for error in sending command @@ -379,13 +381,20 @@ asynStatus SM300Controller::poll() setIntegerParam(error_code_, code); skip: - has_error_ = motorInError || comStatus != asynSuccess; + has_error_ = comStatus != asynSuccess; - for (int i=0; i < this->numAxes_; i++) { + for (int i = 0; i < this->numAxes_; i++) { axis = this->getAxis(i); - axis->setIntegerParam(this->motorStatusProblem_, axis->has_error() || has_error_ ? 1 : 0); - axis->callParamCallbacks(); - } + // make sure status is set for refresh during first 10 polls otherwise it is set on start + if (polls_count_ < REFRESH_ERROR_FOR_POLL_COUNTS) { + axis->setIntegerParam(this->motorStatusCommsError_, axis->has_error() || has_error_ ? 0 : 1); + polls_count_++; + } + axis->setIntegerParam(this->motorStatusCommsError_, axis->has_error() || has_error_ ? 1 : 0); + axis->setIntegerParam(this->motorStatusProblem_, motorHasError ? 1 : 0); + axis->callParamCallbacks(); + } + callParamCallbacks(); return comStatus ? asynError : asynSuccess; } @@ -453,7 +462,7 @@ asynStatus SM300Axis::poll(bool *moving) skip: has_error_ = comStatus != asynSuccess; - setIntegerParam(pC_->motorStatusProblem_, has_error_ || pC_->has_error() ? 1 : 0); + setIntegerParam(pC_->motorStatusCommsError_, has_error_ || pC_->has_error() ? 1 : 0); callParamCallbacks(); return comStatus ? asynError : asynSuccess; } diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 716e722b3..9bc5d6cdc 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -60,6 +60,11 @@ class SM300Controller : public asynMotorController { bool home_axis_x_; bool home_axis_y_; + int polls_count_; + + // number of times the error flag should be refreshed manually. 10 is arbitary bt works. + const int REFRESH_ERROR_FOR_POLL_COUNTS = 10; + #define FIRST_SM300_PARAM reset_ int reset_; int reset_and_home_; From 6a35b47400ac43eeafae1a68896cae405aee1018 Mon Sep 17 00:00:00 2001 From: Holt Date: Tue, 30 Jan 2018 09:44:56 +0000 Subject: [PATCH 120/232] Downgrade for 2010 --- motorApp/SM300Src/SM300Driver.cpp | 2 +- motorApp/SM300Src/SM300Driver.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 478047c6a..9ca27f791 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -505,7 +505,7 @@ asynStatus SM300Axis::move(double position, int relative, double minVelocity, do return asynError; } - sprintf(temp, "B%c%.0f", axisLabel, round(move_to)); + sprintf(temp, "B%c%.0f", axisLabel, floor(move_to + 0.5)); comStatus = pC_->sendCommand(temp); if (comStatus) goto skip; diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 9bc5d6cdc..67223c30e 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -63,7 +63,7 @@ class SM300Controller : public asynMotorController { int polls_count_; // number of times the error flag should be refreshed manually. 10 is arbitary bt works. - const int REFRESH_ERROR_FOR_POLL_COUNTS = 10; + static const int REFRESH_ERROR_FOR_POLL_COUNTS = 10; #define FIRST_SM300_PARAM reset_ int reset_; From a1d871610b096a1dd2e9bd84d8e8e30c52109b36 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 30 Jan 2018 13:29:59 +0000 Subject: [PATCH 121/232] Follow gcc rules --- motorApp/SM300Src/SM300Driver.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index 9ca27f791..f0e2d8569 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -32,17 +32,16 @@ Based on the SM100 Model 3 device driver */ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName, int numAxes, double movingPollPeriod, double idlePollPeriod) - : is_moving_(false), - polls_count_(0), - asynMotorController(portName, numAxes, NUM_SM300_PARAMS, + : asynMotorController(portName, numAxes, NUM_SM300_PARAMS, 0, // No additional interfaces beyond those in base class 0, // No additional callback interfaces beyond those in base class ASYN_CANBLOCK | ASYN_MULTIDEVICE, 1, // autoconnect - 0, 0) // Default priority and stack size + 0, 0), // Default priority and stack size + is_moving_(false), + polls_count_(0) { asynStatus status; - SM300Axis *pAxis; static const char *functionName = "SM300Controller::SM300Controller"; /* Connect to SM300 controller */ @@ -55,8 +54,8 @@ SM300Controller::SM300Controller(const char *portName, const char *SM300PortName if (numAxes != 2) { errlogPrintf("SM300: Driver is only setup for two axes X and Y!\n"); } - pAxis = new SM300Axis(this, 0, 'X'); - pAxis = new SM300Axis(this, 1, 'Y'); + new SM300Axis(this, 0, 'X'); + new SM300Axis(this, 1, 'Y'); createParam(SM300ResetString, asynParamInt32, &reset_); createParam(SM300ResetAndHomeString, asynParamInt32, &reset_and_home_); @@ -300,6 +299,7 @@ asynStatus SM300Controller::poll() asynStatus comStatus; SM300Axis *axis; bool motorHasError = false; + int code; // Read the current status of the motor (at Poition, Not at position, Error) comStatus = this->sendQuery("LM", true); @@ -361,7 +361,7 @@ asynStatus SM300Controller::poll() errlogPrintf("SM300 error code: return string is too short.\n"); goto skip; } - int code = strtol(&this->inString_[2], NULL, 16); + code = strtol(&this->inString_[2], NULL, 16); if (code != 0) { motorHasError = true; @@ -392,7 +392,7 @@ asynStatus SM300Controller::poll() } axis->setIntegerParam(this->motorStatusCommsError_, axis->has_error() || has_error_ ? 1 : 0); axis->setIntegerParam(this->motorStatusProblem_, motorHasError ? 1 : 0); - axis->callParamCallbacks(); + axis->callParamCallbacks(); } callParamCallbacks(); From b4eb5845626bf3dd73a269acd20d6247e79505ac Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Tue, 30 Jan 2018 14:04:45 +0000 Subject: [PATCH 122/232] Fix duplicate fields --- motorApp/Db/SM300_extra.db | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/motorApp/Db/SM300_extra.db b/motorApp/Db/SM300_extra.db index 73f2c13f4..924e90953 100644 --- a/motorApp/Db/SM300_extra.db +++ b/motorApp/Db/SM300_extra.db @@ -87,9 +87,9 @@ record(mbbi, "$(P)ERROR") { field(ELSV, "MAJOR") field(ELVL, "11") - field(TWST, "Unknown") - field(TWSV, "MAJOR") - field(TWVL, "12") + field(TVST, "Unknown") + field(TVSV, "MAJOR") + field(TVVL, "12") field(TTST, "Cmd rep x4 no success") field(TTSV, "MAJOR") From 6d54d94ea3e6bd6ef01bcd148ac560fcaa335a1a Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Tue, 27 Feb 2018 13:29:41 +0000 Subject: [PATCH 123/232] Updated build dependencies --- configure/RELEASE | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/configure/RELEASE b/configure/RELEASE index 524672e89..dd3eb6dfa 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -1,8 +1,10 @@ -# top level master release and local private options -include $(TOP)/../../../configure/MASTER_RELEASE --include $(TOP)/../../../configure/MASTER_RELEASE.$(EPICS_HOST_ARCH) --include $(TOP)/../../../configure/MASTER_RELEASE.private --include $(TOP)/../../../configure/MASTER_RELEASE.private.$(EPICS_HOST_ARCH) +ASYN=$(SUPPORT)/asyn/master +AUTOSAVE=$(SUPPORT)/autosave/master +BUSY=$(SUPPORT)/busy/master +CALC=$(SUPPORT)/calc/master +IPAC=$(SUPPORT)/ipac/master +SNCSEQ=$(SUPPORT)/seq/master +SSCAN=$(SUPPORT)/sscan/master # optional extra local definitions here -include $(TOP)/configure/RELEASE.private From 217fe88f508d9279aa3534f89fc1c4e1027f1494 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 9 Mar 2018 13:08:42 +0000 Subject: [PATCH 124/232] Add missing ONCRPC --- configure/RELEASE | 1 + 1 file changed, 1 insertion(+) diff --git a/configure/RELEASE b/configure/RELEASE index dd3eb6dfa..915135a09 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -5,6 +5,7 @@ CALC=$(SUPPORT)/calc/master IPAC=$(SUPPORT)/ipac/master SNCSEQ=$(SUPPORT)/seq/master SSCAN=$(SUPPORT)/sscan/master +ONCRPC=$(SUPPORT)/oncrpc/master # optional extra local definitions here -include $(TOP)/configure/RELEASE.private From 95e411073761b820c970a0c156d41594e1ae1016 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Tue, 10 Apr 2018 12:46:32 +0100 Subject: [PATCH 125/232] Added polling to the linmot --- motorApp/Db/linmot_extra.db | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/motorApp/Db/linmot_extra.db b/motorApp/Db/linmot_extra.db index a8331a79e..52cb26863 100644 --- a/motorApp/Db/linmot_extra.db +++ b/motorApp/Db/linmot_extra.db @@ -1,3 +1,11 @@ record(motor, "$(P)$(M)") { field(OFF, "$(OFF)") } + +record(ao, "$(P)$(M):SCAN") { + field(SCAN, "1 second") + field(DTYP, "Soft Channel") + field(DESC, "Polls the motor position") + field(OUT, "$(P)$(M).STUP PP") + field(OVAL, "1") +} From 2b154ffe0b1235ae44ad6a5f64a14c24c82d3cb9 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Tue, 10 Apr 2018 17:18:28 +0100 Subject: [PATCH 126/232] Added db for periodic motor polling --- motorApp/Db/Makefile | 1 + motorApp/Db/linmot_extra.db | 8 -------- motorApp/Db/periodic_polling.db | 7 +++++++ 3 files changed, 8 insertions(+), 8 deletions(-) create mode 100644 motorApp/Db/periodic_polling.db diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index 633c971fb..9baf633df 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -47,6 +47,7 @@ DB += PI_Support.db PI_SupportCtrl.db DB += Phytron_motor.db Phytron_I1AM01.db Phytron_MCM01.db DB += linmot_extra.db DB += SM300_extra.db +DB += periodic_polling.db #---------------------------------------------------- # Declare template files which do not show up in DB #USES_TEMPLATE += motor.template diff --git a/motorApp/Db/linmot_extra.db b/motorApp/Db/linmot_extra.db index 52cb26863..a8331a79e 100644 --- a/motorApp/Db/linmot_extra.db +++ b/motorApp/Db/linmot_extra.db @@ -1,11 +1,3 @@ record(motor, "$(P)$(M)") { field(OFF, "$(OFF)") } - -record(ao, "$(P)$(M):SCAN") { - field(SCAN, "1 second") - field(DTYP, "Soft Channel") - field(DESC, "Polls the motor position") - field(OUT, "$(P)$(M).STUP PP") - field(OVAL, "1") -} diff --git a/motorApp/Db/periodic_polling.db b/motorApp/Db/periodic_polling.db new file mode 100644 index 000000000..961d89bc3 --- /dev/null +++ b/motorApp/Db/periodic_polling.db @@ -0,0 +1,7 @@ +record(ao, "$(P)$(M):SCAN") { + field(SCAN, "1 second") + field(DTYP, "Soft Channel") + field(DESC, "Polls the motor position") + field(OUT, "$(P)$(M).STUP PP") + field(OVAL, "1") +} From 581d9ec529de8580e9bf7bad23b6eebbe095595d Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Fri, 13 Apr 2018 10:56:44 +0100 Subject: [PATCH 127/232] Slowed polling down to avoid issues found with too much comms traffic on LINMOT --- motorApp/Db/periodic_polling.db | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/Db/periodic_polling.db b/motorApp/Db/periodic_polling.db index 961d89bc3..ceb0554a2 100644 --- a/motorApp/Db/periodic_polling.db +++ b/motorApp/Db/periodic_polling.db @@ -1,5 +1,5 @@ record(ao, "$(P)$(M):SCAN") { - field(SCAN, "1 second") + field(SCAN, "10 second") field(DTYP, "Soft Channel") field(DESC, "Polls the motor position") field(OUT, "$(P)$(M).STUP PP") From 93f4a0707a7cc2d58fc3b066f6209ed31ff9026d Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 7 Jun 2018 14:02:52 +0100 Subject: [PATCH 128/232] Disable position maintenance --- motorApp/MotorSrc/motorRecord.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MotorSrc/motorRecord.cc b/motorApp/MotorSrc/motorRecord.cc index 4a9b73eeb..cb81c5f52 100644 --- a/motorApp/MotorSrc/motorRecord.cc +++ b/motorApp/MotorSrc/motorRecord.cc @@ -3415,7 +3415,7 @@ static void monitor(motorRecord * pmr) UNMARK(M_MSTA); if (msta.Bits.GAIN_SUPPORT) { - unsigned short pos_maint = (msta.Bits.EA_POSITION) ? 1 : 0; + unsigned short pos_maint = 0; /*(msta.Bits.EA_POSITION) ? 1 : 0;*/ if (pos_maint != pmr->cnen) { pmr->cnen = pos_maint; From 4ed144231274caa4169cf9f4d306c14f86215e49 Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Tue, 22 May 2018 13:33:07 +0100 Subject: [PATCH 129/232] Stop-reset-stop to cover stop under normal mode and tracking abort --- motorApp/MclennanSrc/devPM304.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 5e6d9bff3..6e16117e9 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -289,10 +289,12 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo of all motors */ break; case STOP_AXIS: - /* Send a reset before the stop command so that it's not impeded by a tracking abort */ + /* Send a stop command as our first port of call */ + sprintf(buff, "%dST;", axis); + strcat(motor_call->message, buff); + /* Send a second stop preceded by a reset command as a contingency in case the first stop is impeded by a tracking abort */ sprintf(buff, "%dRS;", axis); strcat(motor_call->message, buff); - sprintf(buff, "%dST;", axis); break; case JOG: From 795c20d36f164a99897cbe13b13851ea41c8dbc2 Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Mon, 21 May 2018 11:32:31 +0100 Subject: [PATCH 130/232] Only print in debug mode --- motorApp/MclennanSrc/homing.st | 161 +++++++++++++++++---------------- 1 file changed, 82 insertions(+), 79 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 70069432a..bb8a1a7ca 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -29,116 +29,119 @@ ss motor { state init { - when () - { - /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0 */ - mode = atoi(macValueGet("MODE")); - axis = atoi(macValueGet("AXIS")); - debug = atoi(macValueGet("DEBUG")); - printf("Sequencer: Homing mode for axis %i set to %i\n", axis, mode); - if (debug) { - printf("Sequencer: Debug mode ON"); - } - } state ready + when () + { + /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0, 3: Constant veloctiy move and 0 */ + mode = atoi(macValueGet("MODE")); + axis = atoi(macValueGet("AXIS")); + debug = atoi(macValueGet("DEBUG")); + printf("Sequencer: Homing mode for axis %i set to %i\n", axis, mode); + if (debug) { + printf("Sequencer: Debug mode ON"); + } + } state ready } state ready - { + { when (home_reverse_pv==1) { - if (debug) { - printf("Sequencer: FROM ready TO reverse_home_requested\n"); - } + if (debug) { + printf("Sequencer: FROM ready TO reverse_home_requested\n"); + } } state reverse_home_requested - + when (home_forward_pv==1) { - if (debug) { - printf("Sequencer: FROM ready TO forward_home_requested\n"); - } + if (debug) { + printf("Sequencer: FROM ready TO forward_home_requested\n"); + } } state forward_home_requested } state forward_home_requested { - /* In mode 1 we need to wait for the home to be cancelled before requesting a jog */ - when (home_forward_pv==0 && mode==1) { - jog_forward_value = 1; - pvPut(jog_forward_value); - if (debug) { - printf("Sequencer: FROM forward_home_requested TO processing_move_request (mode 1)\n"); - } - } state processing_move_request - + /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ + when (home_forward_pv==0 && (mode==1 || mode==3)) { + jog_forward_value = 1; + pvPut(jog_forward_value); + if (debug) { + printf("Sequencer: FROM forward_home_requested TO processing_move_request (constant velocity move)\n"); + } + } state processing_move_request + /* No delay needed for modes other than 1 */ - when (mode!=1) { - if (debug) { - printf("Sequencer: FROM forward_home_requested TO processing_move_request\n"); - } - } state processing_move_request + when (!(mode==1 || mode==3)) { + if (debug) { + printf("Sequencer: FROM forward_home_requested TO processing_move_request\n"); + } + } state processing_move_request } state reverse_home_requested { - /* In mode 1 we need to wait for the home to be cancelled before requesting a jog */ - when (mode==1 && home_reverse_pv==0) { - jog_reverse_value = 1; - pvPut(jog_reverse_value); - if (debug) { - printf("Sequencer: FROM reverse_home_requested TO processing_move_request (mode 1)\n"); - } - } state processing_move_request - - /* No delay needed for modes other than 1 */ - when (mode!=1) { - if (debug) { - printf("Sequencer: FROM reverse_home_requested TO processing_move_request\n"); - } - } state processing_move_request + /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ + when ((mode==1 || mode==3) && home_reverse_pv==0) { + jog_reverse_value = 1; + pvPut(jog_reverse_value); + if (debug) { + printf("Sequencer: FROM reverse_home_requested TO processing_move_request (constant velocity move)\n"); + } + } state processing_move_request + + /* No delay needed for modes other than 1 and 3 */ + when (!(mode==1 || mode==3)) { + if (debug) { + printf("Sequencer: FROM reverse_home_requested TO processing_move_request\n"); + } + } state processing_move_request } state processing_move_request { - when (movable==0) { - if (debug) { - printf("Sequencer: FROM processing_move_request TO moving\n"); - } - } state moving + when (movable==0) { + if (debug) { + printf("Sequencer: FROM processing_move_request TO moving\n"); + } + } state moving } state moving - { + { when (movable==1) { - if ( mode==2 ) { - set = 1; - position = 0; - position_d = 0; - - pvPut(set); - pvPut(position); - pvPut(position_d); - - set = 0; - pvPut(set); - } - if (debug) { - printf("Sequencer: FROM moving TO done\n"); - } + if ( mode==2 || mode==3 ) { + if (debug) { + printf("Sequencer: Setting position to zero (modes 2 and 3)\n"); + } + set = 1; + position = 0; + position_d = 0; + + pvPut(set); + pvPut(position); + pvPut(position_d); + + set = 0; + pvPut(set); + } + if (debug) { + printf("Sequencer: FROM moving TO done\n"); + } } state done } state done { when () { - if ( mode==1 ) { - jog_reverse_value = 0; - pvPut(jog_reverse_value); - jog_forward_value = 0; - pvPut(jog_forward_value); - } - if (debug) { - printf("Sequencer: FROM done TO ready\n"); - } - } state ready + if ( (mode==1 || mode==3) ) { + jog_reverse_value = 0; + pvPut(jog_reverse_value); + jog_forward_value = 0; + pvPut(jog_forward_value); + } + if (debug) { + printf("Sequencer: FROM done TO ready\n"); + } + } state ready } } From 4722c8fd7edca33e5a41cd6d79e681f916e379ee Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Mon, 21 May 2018 11:43:37 +0100 Subject: [PATCH 131/232] Remove some unecessary parentheses --- motorApp/MclennanSrc/homing.st | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index bb8a1a7ca..6b140748e 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -133,7 +133,7 @@ ss motor state done { when () { - if ( (mode==1 || mode==3) ) { + if (mode==1 || mode==3) { jog_reverse_value = 0; pvPut(jog_reverse_value); jog_forward_value = 0; From 8d6d479968d01c3d98fbeb6c5743c9660ad27e39 Mon Sep 17 00:00:00 2001 From: Adrian Potter Date: Mon, 21 May 2018 11:45:04 +0100 Subject: [PATCH 132/232] Be consistent on white space --- motorApp/MclennanSrc/homing.st | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 6b140748e..66cb1342d 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -110,7 +110,7 @@ ss motor { when (movable==1) { - if ( mode==2 || mode==3 ) { + if (mode==2 || mode==3) { if (debug) { printf("Sequencer: Setting position to zero (modes 2 and 3)\n"); } From 553e0b7bdba5e1b5c830e7747c61463e9530a4e9 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 12 Jul 2018 21:10:17 +0100 Subject: [PATCH 133/232] Update comment --- motorApp/Db/motorUtil.db | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 233b5c8ea..56394079d 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -52,9 +52,9 @@ record(calcout, "$(P)_MOVING") ## it is OK to reuse OUTA in the lines when they refer to a ## specific IOC and no two lines will get loaded at one. However ## it is IMPORTANT to not re-use one of the outputs on the -## MOVING or _MOVING calc record target -## galils done in a slightly different way for historical reasons, -## will be fixed in upcoming ticket. +## MOVING or _MOVING1 etc calc record target +## don't write to the L fields of MOVING, _MOVING1 and _MOVING2 +## as this is written to already - see INSTETC.db record(dfanout, "$(P)_FAN") { field(OMSL, "supervisory") From 5089d8cfb0b48cb59b5889cbd123ab66dc430800 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 12 Jul 2018 21:10:45 +0100 Subject: [PATCH 134/232] Re-enable pos_maint --- motorApp/MotorSrc/motorRecord.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MotorSrc/motorRecord.cc b/motorApp/MotorSrc/motorRecord.cc index cb81c5f52..4a9b73eeb 100644 --- a/motorApp/MotorSrc/motorRecord.cc +++ b/motorApp/MotorSrc/motorRecord.cc @@ -3415,7 +3415,7 @@ static void monitor(motorRecord * pmr) UNMARK(M_MSTA); if (msta.Bits.GAIN_SUPPORT) { - unsigned short pos_maint = 0; /*(msta.Bits.EA_POSITION) ? 1 : 0;*/ + unsigned short pos_maint = (msta.Bits.EA_POSITION) ? 1 : 0; if (pos_maint != pmr->cnen) { pmr->cnen = pos_maint; From b6ab2bf1a2df3d2dbaf94f1b81727e43815bf561 Mon Sep 17 00:00:00 2001 From: Alistair McGann Date: Fri, 21 Sep 2018 17:16:42 +0100 Subject: [PATCH 135/232] Added new homing mode --- motorApp/Db/Makefile | 3 ++- motorApp/Db/motor_init_extra.db | 14 ++++++++++ motorApp/MclennanSrc/homing.st | 45 ++++++++++++++++++++------------- 3 files changed, 44 insertions(+), 18 deletions(-) create mode 100644 motorApp/Db/motor_init_extra.db diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index 9baf633df..609eca50f 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -45,7 +45,8 @@ DB += profileMoveControllerXPS.template DB += profileMoveAxisXPS.template DB += PI_Support.db PI_SupportCtrl.db DB += Phytron_motor.db Phytron_I1AM01.db Phytron_MCM01.db -DB += linmot_extra.db +#DB += linmot_extra.db +DB += motor_init_extra.db DB += SM300_extra.db DB += periodic_polling.db #---------------------------------------------------- diff --git a/motorApp/Db/motor_init_extra.db b/motorApp/Db/motor_init_extra.db new file mode 100644 index 000000000..5beb2b2fd --- /dev/null +++ b/motorApp/Db/motor_init_extra.db @@ -0,0 +1,14 @@ +# If more fields need exposing to macros please update +# all substitution files which call this db file. +# +# Some IOCs using this file: +# LINMOT +# MCLEN +# +# Find all by running "grep -rl 'motor_init_extra.db'" from git bash in top level EPICS +# Ignore results which say 'Device or resource busy' +# + +record(motor, "$(P)$(M)") { + field(OFF, "$(OFF)") +} diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 66cb1342d..ebe9f6cb2 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -8,9 +8,13 @@ assign jog_forward_value to "{MOTPV}.JOGF"; assign jog_reverse_value to "{MOTPV}.JOGR"; int set, position, position_d; +double offset; assign set to "{MOTPV}.SET"; assign position to "{MOTPV}.VAL"; assign position_d to "{MOTPV}.DVAL"; +assign offset to "{MOTPV}.OFF"; + +double offsetval; int debug_flag; int debug; @@ -31,7 +35,7 @@ ss motor { when () { - /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0, 3: Constant veloctiy move and 0 */ + /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0, 3: Constant veloctiy move and 0 all */ mode = atoi(macValueGet("MODE")); axis = atoi(macValueGet("AXIS")); debug = atoi(macValueGet("DEBUG")); @@ -62,7 +66,7 @@ ss motor state forward_home_requested { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when (home_forward_pv==0 && (mode==1 || mode==3)) { + when (home_forward_pv==0 && (mode==1 || mode==3 || mode==4)) { jog_forward_value = 1; pvPut(jog_forward_value); if (debug) { @@ -71,7 +75,7 @@ ss motor } state processing_move_request /* No delay needed for modes other than 1 */ - when (!(mode==1 || mode==3)) { + when (!(mode==1 || mode==3 || mode==4)) { if (debug) { printf("Sequencer: FROM forward_home_requested TO processing_move_request\n"); } @@ -81,7 +85,7 @@ ss motor state reverse_home_requested { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when ((mode==1 || mode==3) && home_reverse_pv==0) { + when ((mode==1 || mode==3 || mode==4) && home_reverse_pv==0) { jog_reverse_value = 1; pvPut(jog_reverse_value); if (debug) { @@ -90,7 +94,7 @@ ss motor } state processing_move_request /* No delay needed for modes other than 1 and 3 */ - when (!(mode==1 || mode==3)) { + when (!(mode==1 || mode==3 || mode==4)) { if (debug) { printf("Sequencer: FROM reverse_home_requested TO processing_move_request\n"); } @@ -110,20 +114,27 @@ ss motor { when (movable==1) { - if (mode==2 || mode==3) { + if (mode==2 || mode==3 || mode == 4) { if (debug) { - printf("Sequencer: Setting position to zero (modes 2 and 3)\n"); + printf("Sequencer: Setting position to zero (modes 2, 3, 4)\n"); } - set = 1; - position = 0; - position_d = 0; - - pvPut(set); - pvPut(position); - pvPut(position_d); + + pvGet(offset); + offsetval = offset; + + PVPUT(set, 1); + + PVPUT(position,0); + PVPUT(position_d, 0); + + if (mode==4){ + /* Reapply the offset */ + PVPUT(offset, offsetval); + } + + PVPUT(set, 0); - set = 0; - pvPut(set); + } if (debug) { printf("Sequencer: FROM moving TO done\n"); @@ -133,7 +144,7 @@ ss motor state done { when () { - if (mode==1 || mode==3) { + if (mode==1 || mode==3 || mode==4) { jog_reverse_value = 0; pvPut(jog_reverse_value); jog_forward_value = 0; From dfa2d75a18c5ba4537322f57bea04823034034ca Mon Sep 17 00:00:00 2001 From: Alistair McGann Date: Fri, 21 Sep 2018 17:31:29 +0100 Subject: [PATCH 136/232] Fixed typo --- motorApp/MclennanSrc/homing.st | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index ebe9f6cb2..29d44f68b 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -35,7 +35,7 @@ ss motor { when () { - /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0, 3: Constant veloctiy move and 0 all */ + /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0, 3: Constant veloctiy move and 0 */ mode = atoi(macValueGet("MODE")); axis = atoi(macValueGet("AXIS")); debug = atoi(macValueGet("DEBUG")); From 96abb2ed57ef080201380866953c2d3069c1b9f3 Mon Sep 17 00:00:00 2001 From: Alistair McGann Date: Fri, 21 Sep 2018 17:35:01 +0100 Subject: [PATCH 137/232] Remove commented line --- motorApp/Db/Makefile | 1 - 1 file changed, 1 deletion(-) diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index 609eca50f..b3ed6d2d5 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -45,7 +45,6 @@ DB += profileMoveControllerXPS.template DB += profileMoveAxisXPS.template DB += PI_Support.db PI_SupportCtrl.db DB += Phytron_motor.db Phytron_I1AM01.db Phytron_MCM01.db -#DB += linmot_extra.db DB += motor_init_extra.db DB += SM300_extra.db DB += periodic_polling.db From 54a949c6653cc111881373860954ead4e179e549 Mon Sep 17 00:00:00 2001 From: Alistair McGann Date: Tue, 16 Oct 2018 15:18:15 +0100 Subject: [PATCH 138/232] Combined modes 3 and 4 --- motorApp/MclennanSrc/homing.st | 41 ++++++++++++++++++++-------------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 29d44f68b..0096fcd01 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -8,13 +8,14 @@ assign jog_forward_value to "{MOTPV}.JOGF"; assign jog_reverse_value to "{MOTPV}.JOGR"; int set, position, position_d; -double offset; +double offset, velocity; assign set to "{MOTPV}.SET"; assign position to "{MOTPV}.VAL"; assign position_d to "{MOTPV}.DVAL"; assign offset to "{MOTPV}.OFF"; +assign velocity to "{MOTPV}.VELO"; -double offsetval; +double offsetval, jogvelocity; int debug_flag; int debug; @@ -55,7 +56,15 @@ ss motor } } state reverse_home_requested - when (home_forward_pv==1) + /* Mode 3 always reverse homes */ + when ((home_forward_pv==1) && (mode==3)) + { + if (debug) { + printf("Sequencer: FROM ready TO reverse_home_requested\n"); + } + } state reverse_home_requested + + when ((home_forward_pv==1) && (mode!=3)) { if (debug) { printf("Sequencer: FROM ready TO forward_home_requested\n"); @@ -66,7 +75,7 @@ ss motor state forward_home_requested { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when (home_forward_pv==0 && (mode==1 || mode==3 || mode==4)) { + when (home_forward_pv==0 && (mode==1 || mode==3)) { jog_forward_value = 1; pvPut(jog_forward_value); if (debug) { @@ -75,7 +84,7 @@ ss motor } state processing_move_request /* No delay needed for modes other than 1 */ - when (!(mode==1 || mode==3 || mode==4)) { + when (!(mode==1 || mode==3)) { if (debug) { printf("Sequencer: FROM forward_home_requested TO processing_move_request\n"); } @@ -85,7 +94,7 @@ ss motor state reverse_home_requested { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when ((mode==1 || mode==3 || mode==4) && home_reverse_pv==0) { + when ((mode==1 || mode==3) && home_reverse_pv==0) { jog_reverse_value = 1; pvPut(jog_reverse_value); if (debug) { @@ -94,7 +103,7 @@ ss motor } state processing_move_request /* No delay needed for modes other than 1 and 3 */ - when (!(mode==1 || mode==3 || mode==4)) { + when (!(mode==1 || mode==3)) { if (debug) { printf("Sequencer: FROM reverse_home_requested TO processing_move_request\n"); } @@ -114,9 +123,9 @@ ss motor { when (movable==1) { - if (mode==2 || mode==3 || mode == 4) { + if (mode==2 || mode==3) { if (debug) { - printf("Sequencer: Setting position to zero (modes 2, 3, 4)\n"); + printf("Sequencer: Setting position to zero (modes 2 and 3)\n"); } pvGet(offset); @@ -124,14 +133,12 @@ ss motor PVPUT(set, 1); - PVPUT(position,0); - PVPUT(position_d, 0); - - if (mode==4){ - /* Reapply the offset */ - PVPUT(offset, offsetval); - } + PVPUT(position, 0.0); + PVPUT(position_d, 0.0); + /* Reapply the offset */ + PVPUT(offset, offsetval); + PVPUT(set, 0); @@ -144,7 +151,7 @@ ss motor state done { when () { - if (mode==1 || mode==3 || mode==4) { + if (mode==1 || mode==3) { jog_reverse_value = 0; pvPut(jog_reverse_value); jog_forward_value = 0; From faea6f364d69a9c88228e5ef93c0af90223a9725 Mon Sep 17 00:00:00 2001 From: Alistair McGann Date: Mon, 22 Oct 2018 10:58:24 +0100 Subject: [PATCH 139/232] Mode 3 always reverses to home and jog velocity set on home --- motorApp/MclennanSrc/homing.st | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 0096fcd01..b6f55fc7f 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -8,14 +8,15 @@ assign jog_forward_value to "{MOTPV}.JOGF"; assign jog_reverse_value to "{MOTPV}.JOGR"; int set, position, position_d; -double offset, velocity; +double offset, velocity, jogvelocity; assign set to "{MOTPV}.SET"; assign position to "{MOTPV}.VAL"; assign position_d to "{MOTPV}.DVAL"; assign offset to "{MOTPV}.OFF"; assign velocity to "{MOTPV}.VELO"; +assign jogvelocity to "{MOTPV}.JVEL"; -double offsetval, jogvelocity; +double offsetval; int debug_flag; int debug; @@ -76,6 +77,12 @@ ss motor { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ when (home_forward_pv==0 && (mode==1 || mode==3)) { + + /* Set the jog velocity */ + pvGet(velocity); + jogvelocity = 0.1 * velocity; + pvPut(jogvelocity); + jog_forward_value = 1; pvPut(jog_forward_value); if (debug) { @@ -94,7 +101,13 @@ ss motor state reverse_home_requested { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when ((mode==1 || mode==3) && home_reverse_pv==0) { + when (home_reverse_pv==0 && (mode==1 || mode==3 && home_forward_pv==0)) { + + /* Set the jog velocity */ + pvGet(velocity); + jogvelocity = 0.1 * velocity; + pvPut(jogvelocity); + jog_reverse_value = 1; pvPut(jog_reverse_value); if (debug) { From c701672ec28abdd92b65f526eda7dccdd0ef50fb Mon Sep 17 00:00:00 2001 From: Alistair McGann Date: Mon, 22 Oct 2018 11:43:24 +0100 Subject: [PATCH 140/232] Moved jog velocity setting to init --- motorApp/MclennanSrc/homing.st | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index b6f55fc7f..c610cd28d 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -16,6 +16,9 @@ assign offset to "{MOTPV}.OFF"; assign velocity to "{MOTPV}.VELO"; assign jogvelocity to "{MOTPV}.JVEL"; +string egu; +assign egu to "{MOTPV}.EGU"; + double offsetval; int debug_flag; @@ -42,8 +45,16 @@ ss motor axis = atoi(macValueGet("AXIS")); debug = atoi(macValueGet("DEBUG")); printf("Sequencer: Homing mode for axis %i set to %i\n", axis, mode); + + /* Set the jog velocity */ + pvGet(velocity); + jogvelocity = 0.1 * velocity; + pvPut(jogvelocity); + pvGet(egu); + printf("Sequencer: Setting jog and home velocity to %3.2f %s/s\n",jogvelocity, egu); + if (debug) { - printf("Sequencer: Debug mode ON"); + printf("Sequencer: Debug mode ON\n"); } } state ready } @@ -77,12 +88,7 @@ ss motor { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ when (home_forward_pv==0 && (mode==1 || mode==3)) { - - /* Set the jog velocity */ - pvGet(velocity); - jogvelocity = 0.1 * velocity; - pvPut(jogvelocity); - + jog_forward_value = 1; pvPut(jog_forward_value); if (debug) { @@ -103,11 +109,6 @@ ss motor /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ when (home_reverse_pv==0 && (mode==1 || mode==3 && home_forward_pv==0)) { - /* Set the jog velocity */ - pvGet(velocity); - jogvelocity = 0.1 * velocity; - pvPut(jogvelocity); - jog_reverse_value = 1; pvPut(jog_reverse_value); if (debug) { From 089312241261486dda07f3bdb4bea0a8ff73ce10 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Mon, 17 Dec 2018 14:33:30 +0000 Subject: [PATCH 141/232] set vel in init extra not in snl --- motorApp/Db/motor_init_extra.db | 1 + motorApp/MclennanSrc/homing.st | 14 +------------- 2 files changed, 2 insertions(+), 13 deletions(-) diff --git a/motorApp/Db/motor_init_extra.db b/motorApp/Db/motor_init_extra.db index 5beb2b2fd..8a11766ae 100644 --- a/motorApp/Db/motor_init_extra.db +++ b/motorApp/Db/motor_init_extra.db @@ -11,4 +11,5 @@ record(motor, "$(P)$(M)") { field(OFF, "$(OFF)") + field(JVEL, "$(JVEL)") } diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index c610cd28d..85b2ed737 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -8,16 +8,11 @@ assign jog_forward_value to "{MOTPV}.JOGF"; assign jog_reverse_value to "{MOTPV}.JOGR"; int set, position, position_d; -double offset, velocity, jogvelocity; +double offset; assign set to "{MOTPV}.SET"; assign position to "{MOTPV}.VAL"; assign position_d to "{MOTPV}.DVAL"; assign offset to "{MOTPV}.OFF"; -assign velocity to "{MOTPV}.VELO"; -assign jogvelocity to "{MOTPV}.JVEL"; - -string egu; -assign egu to "{MOTPV}.EGU"; double offsetval; @@ -46,13 +41,6 @@ ss motor debug = atoi(macValueGet("DEBUG")); printf("Sequencer: Homing mode for axis %i set to %i\n", axis, mode); - /* Set the jog velocity */ - pvGet(velocity); - jogvelocity = 0.1 * velocity; - pvPut(jogvelocity); - pvGet(egu); - printf("Sequencer: Setting jog and home velocity to %3.2f %s/s\n",jogvelocity, egu); - if (debug) { printf("Sequencer: Debug mode ON\n"); } From ee6ea830a8a0fb37025e4870213956d9991ce518 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sun, 7 Apr 2019 22:58:30 +0000 Subject: [PATCH 142/232] Remove devSequencer.dbd --- motorExApp/WithAsyn/Makefile | 3 --- 1 file changed, 3 deletions(-) diff --git a/motorExApp/WithAsyn/Makefile b/motorExApp/WithAsyn/Makefile index 58b1ba82a..93aac7a75 100644 --- a/motorExApp/WithAsyn/Makefile +++ b/motorExApp/WithAsyn/Makefile @@ -46,9 +46,6 @@ COMMONDBDS += phytron.dbd DBD += WithAsyn.dbd WithAsyn_DBD += $(COMMONDBDS) -ifdef SNCSEQ -WithAsyn_DBD += devSequencer.dbd -endif DBD += WithAsynVx.dbd WithAsynVx_DBD += $(COMMONDBDS) From 952969b91a62f9cb968f630233e6c2b040475ad4 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 8 Apr 2019 18:31:09 +0000 Subject: [PATCH 143/232] Remove seqDev --- motorExApp/WithAsyn/Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motorExApp/WithAsyn/Makefile b/motorExApp/WithAsyn/Makefile index 93aac7a75..cea001779 100644 --- a/motorExApp/WithAsyn/Makefile +++ b/motorExApp/WithAsyn/Makefile @@ -82,7 +82,7 @@ COMMONLIBS += motor # Needed for Newport SNL programs WithAsyn_LIBS += $(COMMONLIBS) ifdef SNCSEQ -WithAsyn_LIBS += seqDev seq pv +WithAsyn_LIBS += seq pv endif WithAsyn_LIBS += busy WithAsyn_LIBS += asyn @@ -95,7 +95,7 @@ endif WithAsynVx_LIBS += asyn # Needed for Newport SNL programs ifdef SNCSEQ -WithAsynVx_LIBS += seqDev seq pv +WithAsynVx_LIBS += seq pv endif WithAsynVx_LIBS += $(EPICS_BASE_IOC_LIBS) From fb4bde4ebe8c4285199486344a8ba0e0e6edd2ff Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 23 Jul 2019 13:33:19 +0100 Subject: [PATCH 144/232] Need to use /db on windows --- .gitignore | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index 2fd47eefd..04d524a62 100644 --- a/.gitignore +++ b/.gitignore @@ -1,12 +1,15 @@ *~ O.*/ *.swp -/db -/bin -/dbd -/include -/lib -/templates +*BAK.adl +/bin/ +/db/ +/dbd/ +/html/ +/include/ +/lib/ +/templates/ +cdCommands envPaths cdCommands dllPath.bat From c7b539a69e6b6334a1223b77b2f5c45b8e584f6c Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Tue, 6 Aug 2019 18:24:44 +0100 Subject: [PATCH 145/232] Added motor resolution monitors --- motorApp/Db/basic_asyn_motor.db | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/motorApp/Db/basic_asyn_motor.db b/motorApp/Db/basic_asyn_motor.db index 6d6a683c9..25be4ff04 100644 --- a/motorApp/Db/basic_asyn_motor.db +++ b/motorApp/Db/basic_asyn_motor.db @@ -60,3 +60,25 @@ record(ao,"$(P)$(M)Resolution") { field(PREC, "$(PREC)") } +record(ao,"$(P)$(M)_MRESMON") +{ + field(OMSL, "closed_loop") + field(DOL, "$(P)$(M).MRES CP") + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_RESOLUTION") +} + +record(calcout,"$(P)$(M)_MERATMON") +{ + field(INPA, "$(P)$(M).MRES CP") + field(INPB, "$(P)$(M).ERES CP") + field(CALC, "(A != 0 && B != 0) ? A / B : 0") + field(OOPT, "When Non-zero") + field(OUT, "$(P)$(M)_MERAT PP") +} + +record(ao,"$(P)$(M)_MERAT") +{ + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_ENCODER_RATIO") +} From 4acd945c6f968855421ef16b80bda90e0070f2d2 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Tue, 20 Aug 2019 13:27:42 +0100 Subject: [PATCH 146/232] Added enable/disable to basic record --- motorApp/Db/basic_asyn_motor.db | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/motorApp/Db/basic_asyn_motor.db b/motorApp/Db/basic_asyn_motor.db index 25be4ff04..9a2ff12a6 100644 --- a/motorApp/Db/basic_asyn_motor.db +++ b/motorApp/Db/basic_asyn_motor.db @@ -58,6 +58,14 @@ record(ao,"$(P)$(M)Resolution") { field(DTYP, "asynFloat64") field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_REC_RESOLUTION") field(PREC, "$(PREC)") + +# ISIS enable/disable +record(bo, "$(P)$(M)_able") { + field(DESC, "motor enable") + field(PINI, "YES") + field(OUT, "$(P)$(M).DISP") + field(ZNAM, "Enable") + field(ONAM, "Disable") } record(ao,"$(P)$(M)_MRESMON") From 4d9ec381bf9594b45388a9574442d052e2b3d330 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 24 Jul 2019 14:40:10 +0000 Subject: [PATCH 147/232] Add seq --- motorApp/MotorSimSrc/Makefile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/motorApp/MotorSimSrc/Makefile b/motorApp/MotorSimSrc/Makefile index ff6eabb50..50b8b1c3b 100644 --- a/motorApp/MotorSimSrc/Makefile +++ b/motorApp/MotorSimSrc/Makefile @@ -53,6 +53,10 @@ motorSim_LIBS += motor motorSim_LIBS += asyn motorSim_LIBS += autosave motorSim_LIBS += calc sscan +ifdef SNCSEQ +motorSim_LIBS += seq pv +endif + motorSim_LIBS += $(EPICS_BASE_IOC_LIBS) From bf1305cdf98aff24650589b99bb869ac3f3a7a4a Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 26 Jul 2019 12:37:24 +0100 Subject: [PATCH 148/232] Fix stuck simulated move --- motorApp/MotorSimSrc/motorSimDriver.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index 7eeda198d..c5a1ed8e3 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -67,6 +67,8 @@ motorSimAxis::motorSimAxis(motorSimController *pController, int axis, double low nextpoint_.axis[0].p = start; route_ = routeNew( &(this->endpoint_), &pars ); deferred_move_ = 0; + delayedDone_ = 0; + lastDone_ = 1; } @@ -98,6 +100,7 @@ motorSimController::motorSimController(const char *portName, int numAxes, int pr for (axis=0; axismotorPosition_, DEFAULT_START); + setDoubleParam(axis, this->motorPostMoveDelay_, 0.0); } this->motorThread_ = epicsThreadCreate("motorSimThread", From 1a6c062c0d8f70ada0502b684efe93b4234d45b2 Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Wed, 30 Oct 2019 13:55:31 +0000 Subject: [PATCH 149/232] Remove soft limits during home. Verify limit switch is hit at end of home. --- motorApp/MclennanSrc/homing.st | 80 ++++++++++++++++++++++++---------- 1 file changed, 57 insertions(+), 23 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 85b2ed737..4ff00ef9a 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -2,6 +2,9 @@ program homing("MOTPV=xxx,MODE,AXIS,DEBUG") #include "seqPVmacros.h" +%% #include "string.h" +%% #include "errlog.h" + char* SNLtaskName; int jog_forward_value, jog_reverse_value; assign jog_forward_value to "{MOTPV}.JOGF"; @@ -21,14 +24,28 @@ int debug; int mode; int axis; +double cached_soft_high_limit = 0.0; +double cached_soft_low_limit = 0.0; + /* Turn on run-time debug messages */ option +d; +/* Make code reentrant. This is needed to run more than one instance of this program. */ +option +r; + /* PV definitions */ PV(int, home_forward_pv, "{MOTPV}.HOMF", Monitor); PV(int, home_reverse_pv, "{MOTPV}.HOMR", Monitor); PV(int, movable, "{MOTPV}.DMOV", Monitor); +// Soft limits +PV(int, hlm, "{MOTPV}.HLM", Monitor); +PV(int, llm, "{MOTPV}.LLM", Monitor); + +// Physical limit switches +PV(int, hls, "{MOTPV}.HLS", Monitor); +PV(int, lls, "{MOTPV}.LLS", Monitor); + ss motor { state init @@ -39,10 +56,10 @@ ss motor mode = atoi(macValueGet("MODE")); axis = atoi(macValueGet("AXIS")); debug = atoi(macValueGet("DEBUG")); - printf("Sequencer: Homing mode for axis %i set to %i\n", axis, mode); + errlogSevPrintf(errlogInfo, "Sequencer: Homing mode for axis %i set to %i\n", axis, mode); if (debug) { - printf("Sequencer: Debug mode ON\n"); + errlogSevPrintf(errlogInfo, "Sequencer: Debug mode ON\n"); } } state ready } @@ -52,7 +69,7 @@ ss motor when (home_reverse_pv==1) { if (debug) { - printf("Sequencer: FROM ready TO reverse_home_requested\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM ready TO reverse_home_requested\n"); } } state reverse_home_requested @@ -60,16 +77,25 @@ ss motor when ((home_forward_pv==1) && (mode==3)) { if (debug) { - printf("Sequencer: FROM ready TO reverse_home_requested\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM ready TO reverse_home_requested\n"); } } state reverse_home_requested when ((home_forward_pv==1) && (mode!=3)) { if (debug) { - printf("Sequencer: FROM ready TO forward_home_requested\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM ready TO forward_home_requested\n"); } } state forward_home_requested + + exit { + /* Cache soft limits and then remove them */ + cached_soft_high_limit = hlm; + cached_soft_low_limit = llm; + + PVPUT(hlm, 999999999); + PVPUT(llm, -999999999); + } } state forward_home_requested @@ -80,14 +106,14 @@ ss motor jog_forward_value = 1; pvPut(jog_forward_value); if (debug) { - printf("Sequencer: FROM forward_home_requested TO processing_move_request (constant velocity move)\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM forward_home_requested TO processing_move_request (constant velocity move)\n"); } } state processing_move_request /* No delay needed for modes other than 1 */ when (!(mode==1 || mode==3)) { if (debug) { - printf("Sequencer: FROM forward_home_requested TO processing_move_request\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM forward_home_requested TO processing_move_request\n"); } } state processing_move_request } @@ -100,14 +126,14 @@ ss motor jog_reverse_value = 1; pvPut(jog_reverse_value); if (debug) { - printf("Sequencer: FROM reverse_home_requested TO processing_move_request (constant velocity move)\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM reverse_home_requested TO processing_move_request (constant velocity move)\n"); } } state processing_move_request /* No delay needed for modes other than 1 and 3 */ when (!(mode==1 || mode==3)) { if (debug) { - printf("Sequencer: FROM reverse_home_requested TO processing_move_request\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM reverse_home_requested TO processing_move_request\n"); } } state processing_move_request } @@ -116,7 +142,7 @@ ss motor { when (movable==0) { if (debug) { - printf("Sequencer: FROM processing_move_request TO moving\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM processing_move_request TO moving\n"); } } state moving } @@ -127,26 +153,29 @@ ss motor { if (mode==2 || mode==3) { if (debug) { - printf("Sequencer: Setting position to zero (modes 2 and 3)\n"); + errlogSevPrintf(errlogInfo, "Sequencer: Setting position to zero (modes 2 and 3)\n"); } - pvGet(offset); - offsetval = offset; + if (mode==3 && (!hls) && (!lls)) { + errlogSevPrintf(errlogMajor, "Sequencer: Home mode 3 move complete but hardware limit not engaged. Not defining motor position.\n"); + } else { + pvGet(offset); + offsetval = offset; - PVPUT(set, 1); + PVPUT(set, 1); - PVPUT(position, 0.0); - PVPUT(position_d, 0.0); + PVPUT(position, 0.0); + PVPUT(position_d, 0.0); - /* Reapply the offset */ - PVPUT(offset, offsetval); - - PVPUT(set, 0); - + /* Reapply the offset */ + PVPUT(offset, offsetval); + + PVPUT(set, 0); + } } if (debug) { - printf("Sequencer: FROM moving TO done\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM moving TO done\n"); } } state done } @@ -159,8 +188,13 @@ ss motor jog_forward_value = 0; pvPut(jog_forward_value); } + + /* Re-apply cached soft limits */ + PVPUT(hlm, cached_soft_high_limit); + PVPUT(llm, cached_soft_low_limit); + if (debug) { - printf("Sequencer: FROM done TO ready\n"); + errlogSevPrintf(errlogInfo, "Sequencer: FROM done TO ready\n"); } } state ready } From 4d02a2f789ba0fbad6dd5236fe9a22836ea75141 Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Wed, 30 Oct 2019 16:05:47 +0000 Subject: [PATCH 150/232] Remove limit check, limit is not active after home. --- motorApp/MclennanSrc/homing.st | 27 +++++++++++---------------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 4ff00ef9a..a18d1f76a 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -39,8 +39,8 @@ PV(int, home_reverse_pv, "{MOTPV}.HOMR", Monitor); PV(int, movable, "{MOTPV}.DMOV", Monitor); // Soft limits -PV(int, hlm, "{MOTPV}.HLM", Monitor); -PV(int, llm, "{MOTPV}.LLM", Monitor); +PV(double, hlm, "{MOTPV}.HLM", Monitor); +PV(double, llm, "{MOTPV}.LLM", Monitor); // Physical limit switches PV(int, hls, "{MOTPV}.HLS", Monitor); @@ -156,23 +156,18 @@ ss motor errlogSevPrintf(errlogInfo, "Sequencer: Setting position to zero (modes 2 and 3)\n"); } - if (mode==3 && (!hls) && (!lls)) { - errlogSevPrintf(errlogMajor, "Sequencer: Home mode 3 move complete but hardware limit not engaged. Not defining motor position.\n"); - } else { - pvGet(offset); - offsetval = offset; + pvGet(offset); + offsetval = offset; - PVPUT(set, 1); + PVPUT(set, 1); - PVPUT(position, 0.0); - PVPUT(position_d, 0.0); - - /* Reapply the offset */ - PVPUT(offset, offsetval); - - PVPUT(set, 0); - } + PVPUT(position, 0.0); + PVPUT(position_d, 0.0); + /* Reapply the offset */ + PVPUT(offset, offsetval); + + PVPUT(set, 0); } if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: FROM moving TO done\n"); From 0bec81b7fcf46de42ac6fbb4d13527ec8bf26334 Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Wed, 30 Oct 2019 17:47:54 +0000 Subject: [PATCH 151/232] Add better logging --- motorApp/MclennanSrc/homing.st | 43 ++++++++++++++++++++++------------ 1 file changed, 28 insertions(+), 15 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index a18d1f76a..ee2d1f883 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -4,6 +4,7 @@ program homing("MOTPV=xxx,MODE,AXIS,DEBUG") %% #include "string.h" %% #include "errlog.h" +%% #include "float.h" char* SNLtaskName; int jog_forward_value, jog_reverse_value; @@ -24,8 +25,8 @@ int debug; int mode; int axis; -double cached_soft_high_limit = 0.0; -double cached_soft_low_limit = 0.0; +double cached_soft_high_limit; +double cached_soft_low_limit; /* Turn on run-time debug messages */ option +d; @@ -69,7 +70,7 @@ ss motor when (home_reverse_pv==1) { if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM ready TO reverse_home_requested\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO reverse_home_requested\n", axis); } } state reverse_home_requested @@ -77,14 +78,14 @@ ss motor when ((home_forward_pv==1) && (mode==3)) { if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM ready TO reverse_home_requested\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO reverse_home_requested\n", axis); } } state reverse_home_requested when ((home_forward_pv==1) && (mode!=3)) { if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM ready TO forward_home_requested\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO forward_home_requested\n", axis); } } state forward_home_requested @@ -93,8 +94,11 @@ ss motor cached_soft_high_limit = hlm; cached_soft_low_limit = llm; - PVPUT(hlm, 999999999); - PVPUT(llm, -999999999); + if (debug) { + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: Caching limits (high=%f, low=%f)\n", axis, cached_soft_high_limit, cached_soft_low_limit); + } + PVPUT(hlm, DBL_MAX); + PVPUT(llm, -DBL_MAX); } } @@ -106,14 +110,14 @@ ss motor jog_forward_value = 1; pvPut(jog_forward_value); if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM forward_home_requested TO processing_move_request (constant velocity move)\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM forward_home_requested TO processing_move_request (constant velocity move)\n", axis); } } state processing_move_request /* No delay needed for modes other than 1 */ when (!(mode==1 || mode==3)) { if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM forward_home_requested TO processing_move_request\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM forward_home_requested TO processing_move_request\n", axis); } } state processing_move_request } @@ -126,14 +130,14 @@ ss motor jog_reverse_value = 1; pvPut(jog_reverse_value); if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM reverse_home_requested TO processing_move_request (constant velocity move)\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM reverse_home_requested TO processing_move_request (constant velocity move)\n", axis); } } state processing_move_request /* No delay needed for modes other than 1 and 3 */ when (!(mode==1 || mode==3)) { if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM reverse_home_requested TO processing_move_request\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM reverse_home_requested TO processing_move_request\n", axis); } } state processing_move_request } @@ -142,9 +146,15 @@ ss motor { when (movable==0) { if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM processing_move_request TO moving\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM processing_move_request TO moving\n", axis); } } state moving + + /* If move doesn't start within 10 seconds, cancel home (we may be in a state where we can't move) */ + when(delay(10)){ + errlogSevPrintf(errlogMajor, "Sequencer: axis %i: Unable to start homing move. Cancelling home.\n", axis); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM processing_move_request TO done\n", axis); + } state done } state moving @@ -153,7 +163,7 @@ ss motor { if (mode==2 || mode==3) { if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: Setting position to zero (modes 2 and 3)\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: Setting position to zero (modes 2 and 3)\n", axis); } pvGet(offset); @@ -170,7 +180,7 @@ ss motor PVPUT(set, 0); } if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM moving TO done\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM moving TO done\n", axis); } } state done } @@ -184,12 +194,15 @@ ss motor pvPut(jog_forward_value); } + if (debug) { + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: Reapplying cached limits (high=%f, low=%f)\n", axis, cached_soft_high_limit, cached_soft_low_limit); + } /* Re-apply cached soft limits */ PVPUT(hlm, cached_soft_high_limit); PVPUT(llm, cached_soft_low_limit); if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: FROM done TO ready\n"); + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM done TO ready\n", axis); } } state ready } From 4be447b603543994b9f7dab27fcf82db652e5dbc Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Thu, 31 Oct 2019 18:31:04 +0000 Subject: [PATCH 152/232] Don't define position if user cancelled home. --- motorApp/MclennanSrc/homing.st | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index ee2d1f883..bc7208e3a 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -38,6 +38,10 @@ option +r; PV(int, home_forward_pv, "{MOTPV}.HOMF", Monitor); PV(int, home_reverse_pv, "{MOTPV}.HOMR", Monitor); PV(int, movable, "{MOTPV}.DMOV", Monitor); +PV(int, user_stop, "{MOTPV}.STOP", Monitor); + +/* Need to use SNL queue synchronization to get all events here. */ +syncq user_stop 100; // Soft limits PV(double, hlm, "{MOTPV}.HLM", Monitor); @@ -129,6 +133,7 @@ ss motor jog_reverse_value = 1; pvPut(jog_reverse_value); + if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM reverse_home_requested TO processing_move_request (constant velocity move)\n", axis); } @@ -159,6 +164,16 @@ ss motor state moving { + /* Slightly funny syntax - check if variable was updated, if it was, + update user_stop with it's latest value and then check stop condition. */ + when (pvGetQ(user_stop) && user_stop == 1) + { + errlogSevPrintf(errlogMajor, "Sequencer: axis %i: User stop detected during home - will not define position.\n", axis); + if (debug) { + errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM moving TO done\n", axis); + } + } state done + when (movable==1) { if (mode==2 || mode==3) { From 3b1288af505772637451c86724a6f224a1e9552b Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Thu, 31 Oct 2019 18:34:17 +0000 Subject: [PATCH 153/232] Remove physical limit switch PVs, no longer required --- motorApp/MclennanSrc/homing.st | 4 ---- 1 file changed, 4 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index bc7208e3a..7a5155987 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -47,10 +47,6 @@ syncq user_stop 100; PV(double, hlm, "{MOTPV}.HLM", Monitor); PV(double, llm, "{MOTPV}.LLM", Monitor); -// Physical limit switches -PV(int, hls, "{MOTPV}.HLS", Monitor); -PV(int, lls, "{MOTPV}.LLS", Monitor); - ss motor { state init From 4866bc6cbf77fcf54d838fffc7d39d9ada88d160 Mon Sep 17 00:00:00 2001 From: Alistair McGann Date: Fri, 1 Nov 2019 15:20:45 +0000 Subject: [PATCH 154/232] Clear queue before move --- motorApp/MclennanSrc/homing.st | 3 +++ 1 file changed, 3 insertions(+) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 7a5155987..eb23c0ba4 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -149,6 +149,9 @@ ss motor if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM processing_move_request TO moving\n", axis); } + + pvFlushQ(user_stop); + } state moving /* If move doesn't start within 10 seconds, cancel home (we may be in a state where we can't move) */ From 84602baf719627dc424f4eb6486303271c1d4b43 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 1 Nov 2019 18:40:25 +0000 Subject: [PATCH 155/232] Disable position initialisation on startup --- motorApp/MotorSrc/devMotorAsyn.c | 2 +- motorApp/MotorSrc/motordevCom.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/motorApp/MotorSrc/devMotorAsyn.c b/motorApp/MotorSrc/devMotorAsyn.c index ae2ace6be..154d6bec9 100644 --- a/motorApp/MotorSrc/devMotorAsyn.c +++ b/motorApp/MotorSrc/devMotorAsyn.c @@ -171,7 +171,7 @@ static void init_controller(struct motorRecord *pmr, asynUser *pasynUser ) double position = pPvt->status.position; double rdbd = (fabs(pmr->rdbd) < fabs(pmr->mres) ? fabs(pmr->mres) : fabs(pmr->rdbd) ); double encRatio[2] = {pmr->mres, pmr->eres}; - int use_rel = (pmr->rtry != 0 && pmr->rmod != motorRMOD_I && (pmr->ueip || pmr->urip)); + int use_rel = 0; /*(pmr->rtry != 0 && pmr->rmod != motorRMOD_I && (pmr->ueip || pmr->urip));*/ /*Before setting position, set the correct encoder ratio.*/ start_trans(pmr); diff --git a/motorApp/MotorSrc/motordevCom.cc b/motorApp/MotorSrc/motordevCom.cc index b82bc8fa9..cc35536dd 100644 --- a/motorApp/MotorSrc/motordevCom.cc +++ b/motorApp/MotorSrc/motordevCom.cc @@ -196,7 +196,7 @@ motor_init_record_com(struct motorRecord *mr, int brdcnt, struct driver_table *t double ep_mp[2]; /* encoder pulses, motor pulses */ int rtnStat; msta_field msta; - bool use_rel = (mr->rtry != 0 && mr->rmod != motorRMOD_I && (mr->ueip || mr->urip)); + bool use_rel = false; //(mr->rtry != 0 && mr->rmod != motorRMOD_I && (mr->ueip || mr->urip)); /* allocate space for private field - an motor_trans structure */ mr->dpvt = (struct motor_trans *) malloc(sizeof(struct motor_trans)); From 5180d6e1313fc7d9e2c3ec41a250b76a25c4b746 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 30 Mar 2020 11:57:32 +0100 Subject: [PATCH 156/232] Add additional incudes to remove warnings --- motorApp/MclennanSrc/homing.st | 2 ++ 1 file changed, 2 insertions(+) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index eb23c0ba4..de0d82ac1 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -5,6 +5,8 @@ program homing("MOTPV=xxx,MODE,AXIS,DEBUG") %% #include "string.h" %% #include "errlog.h" %% #include "float.h" +%% #include "stdlib.h" +%% #include "math.h" char* SNLtaskName; int jog_forward_value, jog_reverse_value; From 523c1ead95cb1fabbe1862c5fa5b5950220dc3e8 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 23 Mar 2020 17:26:03 +0000 Subject: [PATCH 157/232] Add missing return and param update --- motorApp/Db/SM300_extra.db | 2 -- motorApp/SM300Src/SM300Driver.cpp | 8 +++++--- motorApp/SM300Src/SM300Driver.h | 8 ++++---- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/motorApp/Db/SM300_extra.db b/motorApp/Db/SM300_extra.db index 924e90953..3001424ff 100644 --- a/motorApp/Db/SM300_extra.db +++ b/motorApp/Db/SM300_extra.db @@ -4,7 +4,6 @@ record (busy, "$(P)RESET_AND_HOME") { field(DTYP, "asynInt32") field(DESC,"Reset and home the motor") - field(SCAN, "I/O Intr") field(ZNAM, "Done") field(ONAM, "Reseting") @@ -16,7 +15,6 @@ record (busy, "$(P)RESET_AND_HOME") record(busy,"$(P)RESET") { field(DTYP, "asynInt32") field(DESC,"Reset the motor parameters") - field(SCAN, "I/O Intr") field(ZNAM, "Done") field(ONAM, "Reseting") field(VAL, "0") diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp index f0e2d8569..82c20fd79 100644 --- a/motorApp/SM300Src/SM300Driver.cpp +++ b/motorApp/SM300Src/SM300Driver.cpp @@ -118,6 +118,7 @@ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { } else if (function == disconnect_) { setIntegerParam(disconnect_, 1); + callParamCallbacks(); setTerminationChars("\x06", 1, "\x04", 1); sprintf(this->outString_, "\x06\x02%s", "M77"); status = writeController(); @@ -127,7 +128,7 @@ asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { else if (function == reset_and_home_) { setIntegerParam(reset_and_home_, 1); callParamCallbacks(); - perform_reset(); + status = perform_reset(); for (int i = 0; i < numAxes_; i++) { getAxis(i)->home(0, 0, 0, 0); } @@ -151,11 +152,12 @@ asynStatus SM300Controller::perform_reset() { //send empty string with ACK to clear the buffer asynStatus status = this->writeController(); + if (status) return status; //set termination character is EOT without check sum * // when sending send incase this mode is switched on setTerminationChars("\x06", 1, "\x04\x0D", 2); - if (sendCommand("PEK0")) return status; + if ( (status = sendCommand("PEK0")) ) return status; setTerminationChars("\x06", 1, "\x04", 1); const char * commands[] = { @@ -221,7 +223,7 @@ asynStatus SM300Controller::perform_reset() { int n_array = (sizeof(commands) / sizeof(const char *)); for (int i = 0; i < n_array; i++) { - if (sendCommand(commands[i])) return status; + if ( (status = sendCommand(commands[i])) ) return status; } setIntegerParam(reset_, 0); diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h index 67223c30e..2f7dc8ca6 100644 --- a/motorApp/SM300Src/SM300Driver.h +++ b/motorApp/SM300Src/SM300Driver.h @@ -66,10 +66,10 @@ class SM300Controller : public asynMotorController { static const int REFRESH_ERROR_FOR_POLL_COUNTS = 10; #define FIRST_SM300_PARAM reset_ - int reset_; - int reset_and_home_; - int disconnect_; - int error_code_; + int reset_; // int param + int reset_and_home_; // int param + int disconnect_; // int param + int error_code_; // int param #define LAST_SM300_PARAM error_code_ From 610fcb9ccb5a5c2143261e046f2831359b748915 Mon Sep 17 00:00:00 2001 From: John-Holt-Tessella Date: Mon, 21 Sep 2020 17:16:23 +0100 Subject: [PATCH 158/232] Add entry for refl server --- motorApp/Db/motorUtil.db | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 56394079d..f3f5a5e4b 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -80,6 +80,7 @@ $(IFIOC_LINMOT_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.H CA") $(IFIOC_BKHOFF_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.I CA") $(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") $(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") +## _MOVING2.A is used by reflectometry ioc } # force periodic processing to catch up any missing, shouldn't From 6d264952f04cb4a47734a204b29d039961a35189 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Tue, 8 Dec 2020 15:48:10 +0000 Subject: [PATCH 159/232] Added newport to is moving flag --- motorApp/Db/motorUtil.db | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index f3f5a5e4b..2eac7dde0 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -81,6 +81,7 @@ $(IFIOC_BKHOFF_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.I CA") $(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") $(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") ## _MOVING2.A is used by reflectometry ioc +$(IFIOC_NWPRTXPS_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.B CA") } # force periodic processing to catch up any missing, shouldn't From 6a33611c959be616214d5339dfc79db4c5e546bc Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sun, 7 Mar 2021 19:31:11 +0000 Subject: [PATCH 160/232] Simulate encoder --- motorApp/MotorSimSrc/motorSimDriver.cpp | 11 ++++++++++- motorApp/MotorSimSrc/motorSimDriver.h | 1 + 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index c5a1ed8e3..4912c45f8 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -69,6 +69,7 @@ motorSimAxis::motorSimAxis(motorSimController *pController, int axis, double low deferred_move_ = 0; delayedDone_ = 0; lastDone_ = 1; + setIntegerParam(pC_->motorStatusHasEncoder_, 1); } @@ -364,6 +365,12 @@ asynStatus motorSimAxis::setPosition(double position) return asynSuccess; } +asynStatus motorSimAxis::setEncoderPosition(double position) +{ + // currently we do not track encoder separately but just keep in syn with motor + return asynMotorAxis::setEncoderPosition(position); +} + asynStatus motorSimAxis::config(int hiHardLimit, int lowHardLimit, int home, int start) { hiHardLimit_ = hiHardLimit; @@ -481,9 +488,11 @@ void motorSimAxis::process(double delta ) } lastDone_ = done; + double encRatio; + pC_->getDoubleParam(axisNo_, pC_->motorEncoderRatio_, &encRatio); setDoubleParam (pC_->motorPosition_, (nextpoint_.axis[0].p+enc_offset_)); - setDoubleParam (pC_->motorEncoderPosition_, (nextpoint_.axis[0].p+enc_offset_)); + setDoubleParam (pC_->motorEncoderPosition_, (nextpoint_.axis[0].p+enc_offset_) * encRatio); setIntegerParam(pC_->motorStatusDirection_, (nextpoint_.axis[0].v > 0)); setIntegerParam(pC_->motorStatusDone_, done); setIntegerParam(pC_->motorStatusHighLimit_, (nextpoint_.axis[0].p >= hiHardLimit_)); diff --git a/motorApp/MotorSimSrc/motorSimDriver.h b/motorApp/MotorSimSrc/motorSimDriver.h index ee04095e9..78a3e75b5 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.h +++ b/motorApp/MotorSimSrc/motorSimDriver.h @@ -31,6 +31,7 @@ class epicsShareClass motorSimAxis : public asynMotorAxis asynStatus stop(double acceleration); asynStatus poll(bool *moving); asynStatus setPosition(double position); + asynStatus setEncoderPosition(double position); asynStatus setHighLimit(double highLimit); asynStatus setLowLimit(double lowLimit); From cab4113edc5b263643d42d89c9f5964e895a232f Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sun, 7 Mar 2021 19:32:04 +0000 Subject: [PATCH 161/232] Exclude dllpath --- .gitignore | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 04d524a62..5f4e4c61b 100644 --- a/.gitignore +++ b/.gitignore @@ -11,8 +11,7 @@ O.*/ /templates/ cdCommands envPaths -cdCommands -dllPath.bat +dllPath*.bat runIOC.bat runIOC.sh relPaths.sh From 44addddfe85cd0dbe21b4c03775f077db710e498 Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Mon, 8 Mar 2021 11:50:00 +0000 Subject: [PATCH 162/232] Add home mode 4 to mclennan --- motorApp/MclennanSrc/devPM304.cc | 4 +++- motorApp/MclennanSrc/homing.st | 25 ++++++++++++++++--------- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index bcb98c274..eac4af5a8 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -164,10 +164,12 @@ STATIC void request_home(char* buff, int model, int axis, int home_direction, in if (model == MODEL_PM304){ sprintf(buff, "%dIX%d;", axis, home_direction); } else { - int motor_default=0, constant_velocity=1, reverse_and_zero=2; + int motor_default=0, constant_velocity=1, reverse_and_zero=2, constant_velocity_move_and_zero=3, forward_and_zero=4; if ( home_mode==motor_default || home_mode==reverse_and_zero ) { if ( home_mode==reverse_and_zero ) { home_direction = -1; + } else if ( home_mode==forward_and_zero ) { + home_direction = 1; } sprintf(buff, "%dHD%d;", axis, home_direction); } else { diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index de0d82ac1..f24026bbb 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -49,13 +49,20 @@ syncq user_stop 100; PV(double, hlm, "{MOTPV}.HLM", Monitor); PV(double, llm, "{MOTPV}.LLM", Monitor); + +#define HOME_MODE_BUILTIN 0 +#define HOME_MODE_CONST_VELOCITY_MOVE 1 +#define HOME_MODE_REVERSE_HOME_AND_ZERO 2 +#define HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO 3 +#define HOME_MODE_FORWARD_HOME_AND_ZERO 4 + ss motor { state init { when () { - /* 0: Use motor home, 1: Constant velocity move, 2: Reverse home and s position to 0, 3: Constant veloctiy move and 0 */ + /* See definitions of home modes above. */ mode = atoi(macValueGet("MODE")); axis = atoi(macValueGet("AXIS")); debug = atoi(macValueGet("DEBUG")); @@ -77,14 +84,14 @@ ss motor } state reverse_home_requested /* Mode 3 always reverse homes */ - when ((home_forward_pv==1) && (mode==3)) + when ((home_forward_pv==1) && (mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO reverse_home_requested\n", axis); } } state reverse_home_requested - when ((home_forward_pv==1) && (mode!=3)) + when ((home_forward_pv==1) && (mode!=HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO forward_home_requested\n", axis); @@ -107,7 +114,7 @@ ss motor state forward_home_requested { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when (home_forward_pv==0 && (mode==1 || mode==3)) { + when (home_forward_pv==0 && (mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { jog_forward_value = 1; pvPut(jog_forward_value); @@ -117,7 +124,7 @@ ss motor } state processing_move_request /* No delay needed for modes other than 1 */ - when (!(mode==1 || mode==3)) { + when (!(mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM forward_home_requested TO processing_move_request\n", axis); } @@ -127,7 +134,7 @@ ss motor state reverse_home_requested { /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when (home_reverse_pv==0 && (mode==1 || mode==3 && home_forward_pv==0)) { + when (home_reverse_pv==0 && (mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO && home_forward_pv==0)) { jog_reverse_value = 1; pvPut(jog_reverse_value); @@ -138,7 +145,7 @@ ss motor } state processing_move_request /* No delay needed for modes other than 1 and 3 */ - when (!(mode==1 || mode==3)) { + when (!(mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM reverse_home_requested TO processing_move_request\n", axis); } @@ -177,7 +184,7 @@ ss motor when (movable==1) { - if (mode==2 || mode==3) { + if (mode==HOME_MODE_REVERSE_HOME_AND_ZERO || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO || mode==HOME_MODE_FORWARD_HOME_AND_ZERO) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: Setting position to zero (modes 2 and 3)\n", axis); } @@ -203,7 +210,7 @@ ss motor state done { when () { - if (mode==1 || mode==3) { + if (mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO) { jog_reverse_value = 0; pvPut(jog_reverse_value); jog_forward_value = 0; From 64954ec19ea8d7960ea993878290d4b93c1c4260 Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Wed, 17 Mar 2021 13:38:31 +0000 Subject: [PATCH 163/232] Update motorApp/MclennanSrc/devPM304.cc Co-authored-by: John Holt --- motorApp/MclennanSrc/devPM304.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index eac4af5a8..7674e5472 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -165,7 +165,7 @@ STATIC void request_home(char* buff, int model, int axis, int home_direction, in sprintf(buff, "%dIX%d;", axis, home_direction); } else { int motor_default=0, constant_velocity=1, reverse_and_zero=2, constant_velocity_move_and_zero=3, forward_and_zero=4; - if ( home_mode==motor_default || home_mode==reverse_and_zero ) { + if ( home_mode==motor_default || home_mode==reverse_and_zero || home_mode==forward_and_zero) { if ( home_mode==reverse_and_zero ) { home_direction = -1; } else if ( home_mode==forward_and_zero ) { @@ -347,4 +347,4 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo Debug(3, "PM304_build_trans: buff=%s, motor_call->message=%s\n", buff, motor_call->message); return (rtnval); -} \ No newline at end of file +} From 2ce0974dc843255c19b8e72d96fd4c87cbd73bd3 Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Wed, 17 Mar 2021 13:44:09 +0000 Subject: [PATCH 164/232] Define home modes as macros --- motorApp/MclennanSrc/devPM304.cc | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 7674e5472..fbee81fae 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -33,6 +33,12 @@ #define STATIC static +#define HOME_MODE_BUILTIN 0 +#define HOME_MODE_CONST_VELOCITY_MOVE 1 +#define HOME_MODE_REVERSE_HOME_AND_ZERO 2 +#define HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO 3 +#define HOME_MODE_FORWARD_HOME_AND_ZERO 4 + extern struct driver_table PM304_access; #define NINT(f) (long)((f)>0 ? (f)+0.5 : (f)-0.5) @@ -164,11 +170,10 @@ STATIC void request_home(char* buff, int model, int axis, int home_direction, in if (model == MODEL_PM304){ sprintf(buff, "%dIX%d;", axis, home_direction); } else { - int motor_default=0, constant_velocity=1, reverse_and_zero=2, constant_velocity_move_and_zero=3, forward_and_zero=4; - if ( home_mode==motor_default || home_mode==reverse_and_zero || home_mode==forward_and_zero) { - if ( home_mode==reverse_and_zero ) { + if ( home_mode==HOME_MODE_BUILTIN || home_mode==HOME_MODE_REVERSE_HOME_AND_ZERO || home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO) { + if ( home_mode==HOME_MODE_REVERSE_HOME_AND_ZERO ) { home_direction = -1; - } else if ( home_mode==forward_and_zero ) { + } else if ( home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO ) { home_direction = 1; } sprintf(buff, "%dHD%d;", axis, home_direction); From 13ed77210aeb6d22b8423fbc8643970c74171212 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Wed, 26 May 2021 20:59:28 +0100 Subject: [PATCH 165/232] Only poll motor when it's not moving --- motorApp/Db/periodic_polling.db | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/motorApp/Db/periodic_polling.db b/motorApp/Db/periodic_polling.db index ceb0554a2..2ce0e5ed9 100644 --- a/motorApp/Db/periodic_polling.db +++ b/motorApp/Db/periodic_polling.db @@ -1,7 +1,8 @@ record(ao, "$(P)$(M):SCAN") { - field(SCAN, "10 second") + field(SCAN, "$(POLL_RATE=10) second") field(DTYP, "Soft Channel") field(DESC, "Polls the motor position") field(OUT, "$(P)$(M).STUP PP") field(OVAL, "1") + field(SDIS, "$(P)$(M).MOVN") } From ac281fa46efef45c8c7f0373414e174215ba9e8c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 25 Jun 2021 13:37:31 +0100 Subject: [PATCH 166/232] Add for newer epics base --- motorApp/MotorSrc/motordrvCom.h | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/MotorSrc/motordrvCom.h b/motorApp/MotorSrc/motordrvCom.h index c2351f5cc..46490d157 100644 --- a/motorApp/MotorSrc/motordrvCom.h +++ b/motorApp/MotorSrc/motordrvCom.h @@ -45,6 +45,7 @@ USAGE... This file contains definitions and structures that #ifndef INCmotordrvComh #define INCmotordrvComh 1 +#include #include #include #include From fc155c1cc97530e27b44cf98cadeac70c29a4c0d Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Mon, 5 Jul 2021 13:08:56 +0100 Subject: [PATCH 167/232] Added initial val on periodic polling PV --- motorApp/Db/periodic_polling.db | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/Db/periodic_polling.db b/motorApp/Db/periodic_polling.db index 2ce0e5ed9..f2fa24f4a 100644 --- a/motorApp/Db/periodic_polling.db +++ b/motorApp/Db/periodic_polling.db @@ -4,5 +4,6 @@ record(ao, "$(P)$(M):SCAN") { field(DESC, "Polls the motor position") field(OUT, "$(P)$(M).STUP PP") field(OVAL, "1") + field(VAL, "1") field(SDIS, "$(P)$(M).MOVN") } From e595f9547fb71d0f50c4148994f27c1fcdf95dd6 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 22 Sep 2021 22:53:11 +0100 Subject: [PATCH 168/232] Add autoonoff fix from recent upstream --- motorApp/MotorSrc/asynMotorAxis.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/motorApp/MotorSrc/asynMotorAxis.cpp b/motorApp/MotorSrc/asynMotorAxis.cpp index 29878b680..e6df34216 100644 --- a/motorApp/MotorSrc/asynMotorAxis.cpp +++ b/motorApp/MotorSrc/asynMotorAxis.cpp @@ -60,6 +60,11 @@ asynMotorAxis::asynMotorAxis(class asynMotorController *pC, int axisNo) // Create the asynUser, connect to this axis pasynUser_ = pasynManager->createAsynUser(NULL, NULL); pasynManager->connectDevice(pasynUser_, pC->portName, axisNo); + + // Initialize some parameters + setIntegerParam(pC_->motorPowerAutoOnOff_, 0); + setDoubleParam(pC_->motorPowerOffDelay_, 0.); + setDoubleParam(pC_->motorPowerOnDelay_, 0.); } asynMotorAxis::~asynMotorAxis() From dcf084c899774125bd0ad4cb5e0c40dfccd5923a Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Wed, 16 Jun 2021 17:52:30 +0100 Subject: [PATCH 169/232] setting creep speed when HD, setting back when velocity is set --- motorApp/MclennanSrc/devPM304.cc | 12 +++++++++--- motorApp/MclennanSrc/drvPM304.cc | 12 ++++++++++++ motorApp/MclennanSrc/drvPM304.h | 1 + 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index fbee81fae..e19ad7ec4 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -166,7 +166,8 @@ STATIC RTN_STATUS PM304_end_trans(struct motorRecord *mr) } /* request homing move */ -STATIC void request_home(char* buff, int model, int axis, int home_direction, int home_mode) { +STATIC void request_home(struct mess_node *motor_call, int model, int axis, int home_direction, int home_mode) { + char buff[30]; if (model == MODEL_PM304){ sprintf(buff, "%dIX%d;", axis, home_direction); } else { @@ -176,11 +177,14 @@ STATIC void request_home(char* buff, int model, int axis, int home_direction, in } else if ( home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO ) { home_direction = 1; } + sprintf(buff, "%dSC%d;", axis, VELO); + strcat(motor_call->message, buff); sprintf(buff, "%dHD%d;", axis, home_direction); } else { // Let SNL take care of everything. See homing.st } } + strcat(motor_call->message, buff); } /* add a part to the transaction */ @@ -256,10 +260,10 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dMR%ld;", axis, ival); break; case HOME_REV: - request_home(buff, cntrl->model, axis, -1, cntrl->home_mode[axis-1]); + request_home(motor_call, cntrl->model, axis, -1, cntrl->home_mode[axis-1]); break; case HOME_FOR: - request_home(buff, cntrl->model, axis, 1, cntrl->home_mode[axis-1]); + request_home(motor_call, cntrl->model, axis, 1, cntrl->home_mode[axis-1]); break; case LOAD_POS: if (cntrl->use_encoder[axis-1]){ @@ -269,6 +273,8 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo case SET_VEL_BASE: break; /* PM304 does not use base velocity */ case SET_VELOCITY: + sprintf(buff, "%dSC%d;", axis, cntrl->creep_speeds[axis-1]); + strcat(motor_call->message, buff); sprintf(buff, "%dSV%ld;", axis, ival); VELO = ival; break; diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index f0812751e..1d2d0069e 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -719,6 +719,18 @@ STATIC int motor_init() cntrl->use_encoder[motor_index] = 1; } } + /* Querying speeds for this axis */ + sprintf(command, "%dQS", motor_index+1); + send_recv_mess(card_index, command ,buff); + /* splice creep speed - split up spaces then parse second integer as creep speed */ + const char s[2] = " "; + char *token; + token = strtok(buff, s); + for (int i=0;i<2;i++) { + token = strtok(NULL, s); + } + int creep_speed = atoi(token); + cntrl->creep_speeds[motor_index] = creep_speed; Debug(3, "PM304 motor_init(), cntrl->model=%d, cntrl->use_encoder[%d]=%d.\n", cntrl->model, motor_index, cntrl->use_encoder[motor_index]); diff --git a/motorApp/MclennanSrc/drvPM304.h b/motorApp/MclennanSrc/drvPM304.h index 58561f667..eb07226a6 100644 --- a/motorApp/MclennanSrc/drvPM304.h +++ b/motorApp/MclennanSrc/drvPM304.h @@ -39,6 +39,7 @@ struct PM304controller int home_mode[PM304_MAX_CHANNELS]; /* The combined home modes for all axes */ char port[80]; /* Asyn port name */ int reset_before_move; /* Reset the controller before any move command */ + int creep_speeds[PM304_MAX_CHANNELS]; /* Creep speed for each axis */ }; RTN_STATUS PM304Setup(int, int); From 3ee39f70c86ec65c9b78800a42c53d0fcb88bad1 Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Mon, 26 Jul 2021 11:19:34 +0100 Subject: [PATCH 170/232] changing token if pm304 --- motorApp/MclennanSrc/drvPM304.cc | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 1d2d0069e..1800c20e4 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -719,11 +719,17 @@ STATIC int motor_init() cntrl->use_encoder[motor_index] = 1; } } + /* Querying speeds for this axis */ sprintf(command, "%dQS", motor_index+1); send_recv_mess(card_index, command ,buff); + char* s; + if (cntrl->model == MODEL_PM304) { + s = "="; + } else { + s = " "; + } /* splice creep speed - split up spaces then parse second integer as creep speed */ - const char s[2] = " "; char *token; token = strtok(buff, s); for (int i=0;i<2;i++) { From ca2a6c168affe2e981ef1399fc6003398cc2a2c7 Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Mon, 26 Jul 2021 12:49:45 +0100 Subject: [PATCH 171/232] set creep speed on both devices when requesting home --- motorApp/MclennanSrc/devPM304.cc | 4 ++-- motorApp/MclennanSrc/drvPM304.cc | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index e19ad7ec4..db5ab5339 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -168,6 +168,8 @@ STATIC RTN_STATUS PM304_end_trans(struct motorRecord *mr) /* request homing move */ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int home_direction, int home_mode) { char buff[30]; + sprintf(buff, "%dSC%d;", axis, VELO); + strcat(motor_call->message, buff); if (model == MODEL_PM304){ sprintf(buff, "%dIX%d;", axis, home_direction); } else { @@ -177,8 +179,6 @@ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int } else if ( home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO ) { home_direction = 1; } - sprintf(buff, "%dSC%d;", axis, VELO); - strcat(motor_call->message, buff); sprintf(buff, "%dHD%d;", axis, home_direction); } else { // Let SNL take care of everything. See homing.st diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 1800c20e4..00c513dec 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -723,13 +723,13 @@ STATIC int motor_init() /* Querying speeds for this axis */ sprintf(command, "%dQS", motor_index+1); send_recv_mess(card_index, command ,buff); + /* splice creep speed - split up spaces then parse second integer as creep speed */ char* s; if (cntrl->model == MODEL_PM304) { s = "="; } else { s = " "; } - /* splice creep speed - split up spaces then parse second integer as creep speed */ char *token; token = strtok(buff, s); for (int i=0;i<2;i++) { From b2a50a8089a28580edea8b3a69475c657e866bb2 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 23 Sep 2021 15:22:30 +0100 Subject: [PATCH 172/232] Parse command line and check for SC just in case output is not what expected --- motorApp/MclennanSrc/devPM304.cc | 14 ++++++++----- motorApp/MclennanSrc/drvPM304.cc | 34 ++++++++++++++++++++------------ 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index db5ab5339..6002d15f3 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -168,9 +168,9 @@ STATIC RTN_STATUS PM304_end_trans(struct motorRecord *mr) /* request homing move */ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int home_direction, int home_mode) { char buff[30]; - sprintf(buff, "%dSC%d;", axis, VELO); - strcat(motor_call->message, buff); if (model == MODEL_PM304){ + sprintf(buff, "%dSC%d;", axis, VELO); + strcat(motor_call->message, buff); sprintf(buff, "%dIX%d;", axis, home_direction); } else { if ( home_mode==HOME_MODE_BUILTIN || home_mode==HOME_MODE_REVERSE_HOME_AND_ZERO || home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO) { @@ -179,9 +179,11 @@ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int } else if ( home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO ) { home_direction = 1; } + sprintf(buff, "%dSC%d;", axis, VELO); + strcat(motor_call->message, buff); sprintf(buff, "%dHD%d;", axis, home_direction); } else { - // Let SNL take care of everything. See homing.st + // Let SNL take care of everything, so do not reset creep speed. See homing.st } } strcat(motor_call->message, buff); @@ -273,8 +275,10 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo case SET_VEL_BASE: break; /* PM304 does not use base velocity */ case SET_VELOCITY: - sprintf(buff, "%dSC%d;", axis, cntrl->creep_speeds[axis-1]); - strcat(motor_call->message, buff); + if (cntrl->creep_speeds[axis-1] != 0) { + sprintf(buff, "%dSC%d;", axis, cntrl->creep_speeds[axis-1]); + strcat(motor_call->message, buff); + } sprintf(buff, "%dSV%ld;", axis, ival); VELO = ival; break; diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 00c513dec..78d9bc8d6 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -722,21 +722,29 @@ STATIC int motor_init() /* Querying speeds for this axis */ sprintf(command, "%dQS", motor_index+1); - send_recv_mess(card_index, command ,buff); + send_recv_mess(card_index, command, buff); /* splice creep speed - split up spaces then parse second integer as creep speed */ - char* s; - if (cntrl->model == MODEL_PM304) { - s = "="; - } else { - s = " "; - } - char *token; - token = strtok(buff, s); - for (int i=0;i<2;i++) { - token = strtok(NULL, s); - } - int creep_speed = atoi(token); + /* P600 returns 01:SC = 700 SV = 16200 SA = 50000 SD = 100000 LD = 200000 + PM304 returns SV=16200,SC=1000,SA=100000,SD=100000 */ + const char* delim = (cntrl->model == MODEL_PM304 ? "=," : "=: "); + char *token, *tok_save = NULL; + int creep_speed = 0; + token = epicsStrtok_r(buff, delim, &tok_save); + do + { + if (!strcmp(token, "SC")) { + token = epicsStrtok_r(NULL, delim, &tok_save); + if (token != NULL) { + creep_speed = atoi(token); + } + } else { + token = epicsStrtok_r(NULL, delim, &tok_save); + } + } while(token != NULL); cntrl->creep_speeds[motor_index] = creep_speed; + if (creep_speed == 0) { + printf("WARNING: unable to determine creep speed for axis %d\n", motor_index); + } Debug(3, "PM304 motor_init(), cntrl->model=%d, cntrl->use_encoder[%d]=%d.\n", cntrl->model, motor_index, cntrl->use_encoder[motor_index]); From 00a8a3ff5ad2258f47ff61c032b65ccbc3ccad02 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 23 Sep 2021 15:33:42 +0100 Subject: [PATCH 173/232] Tidy up --- motorApp/MclennanSrc/drvPM304.cc | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 78d9bc8d6..ab14c4401 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -723,15 +723,13 @@ STATIC int motor_init() /* Querying speeds for this axis */ sprintf(command, "%dQS", motor_index+1); send_recv_mess(card_index, command, buff); - /* splice creep speed - split up spaces then parse second integer as creep speed */ /* P600 returns 01:SC = 700 SV = 16200 SA = 50000 SD = 100000 LD = 200000 PM304 returns SV=16200,SC=1000,SA=100000,SD=100000 */ const char* delim = (cntrl->model == MODEL_PM304 ? "=," : "=: "); char *token, *tok_save = NULL; int creep_speed = 0; token = epicsStrtok_r(buff, delim, &tok_save); - do - { + while(token != NULL) { if (!strcmp(token, "SC")) { token = epicsStrtok_r(NULL, delim, &tok_save); if (token != NULL) { @@ -740,7 +738,7 @@ STATIC int motor_init() } else { token = epicsStrtok_r(NULL, delim, &tok_save); } - } while(token != NULL); + } cntrl->creep_speeds[motor_index] = creep_speed; if (creep_speed == 0) { printf("WARNING: unable to determine creep speed for axis %d\n", motor_index); From 22e0b589c7737f2d69076151bbaa34793bc6f0d7 Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Fri, 1 Oct 2021 15:06:21 +0100 Subject: [PATCH 174/232] set creep speed up to 800 --- motorApp/MclennanSrc/devPM304.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 6002d15f3..bb88d655a 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -167,9 +167,11 @@ STATIC RTN_STATUS PM304_end_trans(struct motorRecord *mr) /* request homing move */ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int home_direction, int home_mode) { + // Max creep speed is 800 - set to velo if under 800 + int creep_speed = (VELO>800) ? 800 : VELO; char buff[30]; if (model == MODEL_PM304){ - sprintf(buff, "%dSC%d;", axis, VELO); + sprintf(buff, "%dSC%d;", axis, creep_speed); strcat(motor_call->message, buff); sprintf(buff, "%dIX%d;", axis, home_direction); } else { @@ -179,7 +181,7 @@ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int } else if ( home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO ) { home_direction = 1; } - sprintf(buff, "%dSC%d;", axis, VELO); + sprintf(buff, "%dSC%d;", axis, creep_speed); strcat(motor_call->message, buff); sprintf(buff, "%dHD%d;", axis, home_direction); } else { From dad364c0117833638551e3c262d1c5c17ac9c053 Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Wed, 27 Oct 2021 10:26:34 +0100 Subject: [PATCH 175/232] Update basic_asyn_motor.db --- motorApp/Db/basic_asyn_motor.db | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/Db/basic_asyn_motor.db b/motorApp/Db/basic_asyn_motor.db index 9a2ff12a6..c34838660 100644 --- a/motorApp/Db/basic_asyn_motor.db +++ b/motorApp/Db/basic_asyn_motor.db @@ -16,6 +16,7 @@ record(motor,"$(P)$(M)") field(DHLM,"$(DHLM)") field(DLLM,"$(DLLM)") field(INIT,"$(INIT)") + field(RTRY,"$(RTRY=10)") field(TWV,"1") # ISIS local archiving and alarm info(archive, "0.02 VAL RBV DVAL OFF MSTA DIR CNEN MOVN DMOV MISS RCNT") From 96e4e78876a325d2f4f16e772c7ba9a4c29d9e37 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 24 Nov 2021 19:13:36 +0000 Subject: [PATCH 176/232] Definitely disable position load on startup --- motorApp/MotorSrc/devMotorAsyn.c | 27 +++++++++++++++++---------- motorApp/MotorSrc/motordevCom.cc | 9 +++++---- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/motorApp/MotorSrc/devMotorAsyn.c b/motorApp/MotorSrc/devMotorAsyn.c index 154d6bec9..16d29f7b9 100644 --- a/motorApp/MotorSrc/devMotorAsyn.c +++ b/motorApp/MotorSrc/devMotorAsyn.c @@ -171,29 +171,36 @@ static void init_controller(struct motorRecord *pmr, asynUser *pasynUser ) double position = pPvt->status.position; double rdbd = (fabs(pmr->rdbd) < fabs(pmr->mres) ? fabs(pmr->mres) : fabs(pmr->rdbd) ); double encRatio[2] = {pmr->mres, pmr->eres}; - int use_rel = 0; /*(pmr->rtry != 0 && pmr->rmod != motorRMOD_I && (pmr->ueip || pmr->urip));*/ - + int use_rel = (pmr->rtry != 0 && pmr->rmod != motorRMOD_I && (pmr->ueip || pmr->urip)); + int dval_non_zero_pos_near_zero = (fabs(pmr->dval) > rdbd) && + (pmr->mres != 0) && (fabs(position * pmr->mres) < rdbd); + int initPos = 0; /*Before setting position, set the correct encoder ratio.*/ start_trans(pmr); build_trans(SET_ENC_RATIO, encRatio, pmr); end_trans(pmr); - if ((use_rel != 0) || - ((fabs(pmr->dval) > rdbd) && (pmr->mres != 0) && (fabs(position * pmr->mres) < rdbd)) - ) + if (initPos) { double setPos = pmr->dval / pmr->mres; epicsEventId initEvent = epicsEventCreate( epicsEventEmpty ); - + RTN_STATUS rtnval; + pPvt->initEvent = initEvent; start_trans(pmr); - build_trans(LOAD_POS, &setPos, pmr); + rtnval = build_trans(LOAD_POS, &setPos, pmr); end_trans(pmr); - asynPrint(pasynUser, ASYN_TRACE_FLOW, - "devMotorAsyn::init_controller, %s set position to %f\n", - pmr->name, setPos ); + if (rtnval != OK) { + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "devMotorAsyn::init_controller, %s failed to set position to %f\n", + pmr->name, setPos ); + } else { + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "devMotorAsyn::init_controller, %s set position to %f\n", + pmr->name, setPos ); + } if ( initEvent ) { diff --git a/motorApp/MotorSrc/motordevCom.cc b/motorApp/MotorSrc/motordevCom.cc index cc35536dd..37bd61a5c 100644 --- a/motorApp/MotorSrc/motordevCom.cc +++ b/motorApp/MotorSrc/motordevCom.cc @@ -189,7 +189,7 @@ motor_init_record_com(struct motorRecord *mr, int brdcnt, struct driver_table *t struct motor_dset *pdset = (struct motor_dset *) (mr->dset); struct board_stat *brdptr; int card, signal; - bool initEncoder, initPos, initString, initPID; + bool initEncoder, initPos = false, initString, initPID; struct motor_trans *ptrans; MOTOR_AXIS_QUERY axis_query; struct mess_node *motor_call; @@ -288,9 +288,10 @@ motor_init_record_com(struct motorRecord *mr, int brdcnt, struct driver_table *t else ep_mp[0] = ep_mp[1] = 1.0; - initPos = ((use_rel == true) || - (fabs(mr->dval) > mr->rdbd && mr->mres != 0 && fabs(axis_query.position * mr->mres) < mr->rdbd) - ) ? true : false; + initPos = false; +// initPos = ((use_rel == true) || +// (fabs(mr->dval) > mr->rdbd && mr->mres != 0 && fabs(axis_query.position * mr->mres) < mr->rdbd) +// ) ? true : false; /* Test for command primitive initialization string. */ initString = (mr->init != NULL && strlen(mr->init)) ? true : false; /* Test for PID support. */ From 816c806bc4819fc050849f7d4e549b51a8a79e98 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Wed, 13 Oct 2021 13:22:54 +0100 Subject: [PATCH 177/232] Allow motors to publish the number of axes they have --- motorApp/Db/motorUtil.db | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 2eac7dde0..d3e030c62 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -116,6 +116,11 @@ record(waveform, "$(P)movingDiff") { field(FTVL, "CHAR") } +record(ai, "$(PVPREFIX)MOT:MTR$(MTRCTRL):AXES_NUM") { + field(DESC, "The num of axes on the controller") + field(VAL, "$(AXES_NUM=8)") +} + #! Further lines contain data used by VisualDCT #! View(50,124,1.3) #! Record("$(P)allstop",100,343,0,0,"$(P)allstop") From ccc79b7dd1a913c067ddebe859152dae33adebb4 Mon Sep 17 00:00:00 2001 From: Dominic Oram Date: Tue, 2 Nov 2021 17:33:54 +0000 Subject: [PATCH 178/232] Added twincat to moving PV --- motorApp/Db/motorUtil.db | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index d3e030c62..1b8248c91 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -82,6 +82,7 @@ $(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") $(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") ## _MOVING2.A is used by reflectometry ioc $(IFIOC_NWPRTXPS_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.B CA") +$(IFIOC_TWINCAT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.C CA") } # force periodic processing to catch up any missing, shouldn't From 8634fc7471b3fc5b117c365c35d725ce9358f0a3 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 26 Nov 2021 18:48:44 +0000 Subject: [PATCH 179/232] Fix negative backlash with relative moves in a negative direction A negative BDST was correctly applied to negative direction moves if moves are absolute. When retries are enabled all moves become relative and backlash was not applied correctly for negative direction moves. --- motorApp/MotorSrc/motorRecord.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MotorSrc/motorRecord.cc b/motorApp/MotorSrc/motorRecord.cc index 3aaf1a321..a59d1ef60 100644 --- a/motorApp/MotorSrc/motorRecord.cc +++ b/motorApp/MotorSrc/motorRecord.cc @@ -2410,7 +2410,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind) } /* IF move is in preferred direction, AND, current position is within backlash range. */ else if ((preferred_dir == true) && - ((use_rel == true && relbpos <= 1.0) || + ((use_rel == true && ((pmr->bdst >= 0 && relbpos <= 1.0) || (pmr->bdst < 0 && relbpos >= 1.0))) || (use_rel == false && (fabs(newpos - currpos) <= rbdst1)) ) ) From efc788cf3fb981f80f9f7dd39de2894a134dcf5c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sun, 28 Nov 2021 18:11:21 +0000 Subject: [PATCH 180/232] Move AXES_NUM to new db --- motorApp/Db/motorController.db | 7 +++++++ motorApp/Db/motorUtil.db | 7 ++----- 2 files changed, 9 insertions(+), 5 deletions(-) create mode 100644 motorApp/Db/motorController.db diff --git a/motorApp/Db/motorController.db b/motorApp/Db/motorController.db new file mode 100644 index 000000000..cfca138eb --- /dev/null +++ b/motorApp/Db/motorController.db @@ -0,0 +1,7 @@ +## pvs that should be available per controller +## Q is normally MOT:MTRxx: + +record(longin, "$(P)$(Q)AXES_NUM") { + field(DESC, "number of axes on controller") + field(VAL, "$(AXES_NUM=8)") +} diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 1b8248c91..e19d2aa7c 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -83,6 +83,8 @@ $(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") ## _MOVING2.A is used by reflectometry ioc $(IFIOC_NWPRTXPS_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.B CA") $(IFIOC_TWINCAT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.C CA") +$(IFIOC_GALILMUL_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.D CA") +$(IFIOC_GALILMUL_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.E CA") } # force periodic processing to catch up any missing, shouldn't @@ -117,11 +119,6 @@ record(waveform, "$(P)movingDiff") { field(FTVL, "CHAR") } -record(ai, "$(PVPREFIX)MOT:MTR$(MTRCTRL):AXES_NUM") { - field(DESC, "The num of axes on the controller") - field(VAL, "$(AXES_NUM=8)") -} - #! Further lines contain data used by VisualDCT #! View(50,124,1.3) #! Record("$(P)allstop",100,343,0,0,"$(P)allstop") From b1a87eb269fb1bdc9a22a718b951de761a038f42 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sun, 28 Nov 2021 18:19:45 +0000 Subject: [PATCH 181/232] Add motorController.db to installs --- motorApp/Db/Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index bd3274eb7..56a34891b 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -39,6 +39,7 @@ DB += ACRAuxBoRBV.template DB += TransPos.db DB += motorUtil.db DB += motorStatus.db +DB += motorController.db DB += asyn_motor.db DB += profileMoveController.template DB += profileMoveAxis.template From f6297206491e0b5637000241596f3fb2dc7acf09 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sat, 4 Dec 2021 00:24:51 +0000 Subject: [PATCH 182/232] More sim diagnostics --- motorApp/MotorSimSrc/motorSimDriver.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index 4912c45f8..0188c881f 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -14,6 +14,7 @@ December 13, 2009 #include #include #include +#include #include #include @@ -277,12 +278,17 @@ asynStatus motorSimAxis::move(double position, int relative, double minVelocity, { route_pars_t pars; static const char *functionName = "move"; + std::cerr << "motorSimAxis::move axis " << axisNo_ << " position " << position << (relative ? " (relative)" : "(absoute)") << " speed min/max " << minVelocity << "/" << maxVelocity << " acceleration " << acceleration << std::endl; 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; + (nextpoint_.axis[0].p <= lowHardLimit_ && position < nextpoint_.axis[0].p) ) + { + std::cerr << "motorSimAxis::move failed (hard limits)" << std::endl; + return asynError; + } if (pC_->movesDeferred_ == 0) { /*Normal move.*/ endpoint_.axis[0].p = position - enc_offset_; @@ -326,6 +332,7 @@ asynStatus motorSimAxis::setVelocity(double velocity, double acceleration ) this->endpoint_.axis[0].p = (this->nextpoint_.axis[0].p + time * ( this->nextpoint_.axis[0].v + 0.5 * deltaV)); this->reroute_ = ROUTE_NEW_ROUTE; + std::cerr << "motorSimAxis::setVelocity axis " << axisNo_ << " velocity " << velocity << " acceleration " << acceleration << std::endl; return asynSuccess; } @@ -335,6 +342,7 @@ asynStatus motorSimAxis::home(double minVelocity, double maxVelocity, double acc asynStatus status = asynError; // static const char *functionName = "home"; + std::cerr << "motorSimAxis::home axis " << axisNo_ << " direction " << (forwards ? "forwards" : "backwards") << std::endl; status = setVelocity((forwards? maxVelocity: -maxVelocity), acceleration ); homing_ = 1; return status; @@ -344,6 +352,7 @@ asynStatus motorSimAxis::home(double minVelocity, double maxVelocity, double acc asynStatus motorSimAxis::moveVelocity(double minVelocity, double velocity, double acceleration ) { asynStatus status = asynError; + std::cerr << "motorSimAxis::moveVelocity axis " << axisNo_ << " velocity " << velocity << " acceleration " << acceleration << std::endl; // static const char *functionName = "moveVelocity"; status = setVelocity(velocity, acceleration ); @@ -354,6 +363,7 @@ asynStatus motorSimAxis::stop(double acceleration ) { // static const char *functionName = "moveVelocityAxis"; + std::cerr << "motorSimAxis::stop axis " << axisNo_ << " acceleration " << acceleration << std::endl; setVelocity(0.0, acceleration ); deferred_move_ = 0; return asynSuccess; @@ -361,12 +371,14 @@ asynStatus motorSimAxis::stop(double acceleration ) asynStatus motorSimAxis::setPosition(double position) { + std::cerr << "motorSimAxis::setPosition axis " << axisNo_ << " position " << position << std::endl; enc_offset_ = position - nextpoint_.axis[0].p; return asynSuccess; } asynStatus motorSimAxis::setEncoderPosition(double position) { + std::cerr << "motorSimAxis::setEncoderPosition axis " << axisNo_ << " position " << position << std::endl; // currently we do not track encoder separately but just keep in syn with motor return asynMotorAxis::setEncoderPosition(position); } From caaa6367fda71ba600ab164b783d7809563eee5c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 6 Dec 2021 12:24:13 +0000 Subject: [PATCH 183/232] Add HOMED status --- motorApp/Db/asyn_motor.db | 14 ++++++++++++++ motorApp/Db/basic_asyn_motor.db | 14 ++++++++++++++ motorApp/Db/basic_motor.db | 14 ++++++++++++++ motorApp/Db/motor.db | 14 ++++++++++++++ motorApp/Db/pseudoMotor.db | 14 ++++++++++++++ 5 files changed, 70 insertions(+) diff --git a/motorApp/Db/asyn_motor.db b/motorApp/Db/asyn_motor.db index 2ace78d36..c36da72f7 100644 --- a/motorApp/Db/asyn_motor.db +++ b/motorApp/Db/asyn_motor.db @@ -93,6 +93,20 @@ record(ao,"$(P)$(M)Resolution") { field(PREC, "$(PREC)") } +# allow splitting out of MSTA +record(mbbiDirect, "$(P)$(M):_MSTABITS") +{ + field(INP, "$(P)$(M).MSTA CP") +} + +record(bi, "$(P)$(M)_HOMED") +{ + field(DESC, "Motor Homed") + field(ZNAM, "No") + field(ONAM, "Yes") + field(INP, "$(P)$(M):_MSTABITS.BE CP") +} + #! Further lines contain data used by VisualDCT #! View(405,484,0.9) #! Record("$(P)$(M)",840,675,0,0,"$(P)$(M)") diff --git a/motorApp/Db/basic_asyn_motor.db b/motorApp/Db/basic_asyn_motor.db index c34838660..ae6c5c0cb 100644 --- a/motorApp/Db/basic_asyn_motor.db +++ b/motorApp/Db/basic_asyn_motor.db @@ -91,3 +91,17 @@ record(ao,"$(P)$(M)_MERAT") field(DTYP, "asynFloat64") field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_ENCODER_RATIO") } + +# allow splitting out of MSTA +record(mbbiDirect, "$(P)$(M):_MSTABITS") +{ + field(INP, "$(P)$(M).MSTA CP") +} + +record(bi, "$(P)$(M)_HOMED") +{ + field(DESC, "Motor Homed") + field(ZNAM, "No") + field(ONAM, "Yes") + field(INP, "$(P)$(M):_MSTABITS.BE CP") +} diff --git a/motorApp/Db/basic_motor.db b/motorApp/Db/basic_motor.db index ef2698749..8b2b47bb3 100644 --- a/motorApp/Db/basic_motor.db +++ b/motorApp/Db/basic_motor.db @@ -22,6 +22,20 @@ grecord(motor,"$(P)$(M)") info(alarm, "Motors") } +# allow splitting out of MSTA +record(mbbiDirect, "$(P)$(M):_MSTABITS") +{ + field(INP, "$(P)$(M).MSTA CP") +} + +record(bi, "$(P)$(M)_HOMED") +{ + field(DESC, "Motor Homed") + field(ZNAM, "No") + field(ONAM, "Yes") + field(INP, "$(P)$(M):_MSTABITS.BE CP") +} + # ISIS local aliases for genie_python alias("$(P)$(M)", "$(P)$(M):SP") alias("$(P)$(M)", "$(P)$(M):SP:RBV") diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index c390754a3..09334be77 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -60,6 +60,20 @@ record(calcout, "$(P)$(M)_twCh") { field(OUT, "$(P)$(M).TWV") } +# allow splitting out of MSTA +record(mbbiDirect, "$(P)$(M):_MSTABITS") +{ + field(INP, "$(P)$(M).MSTA CP") +} + +record(bi, "$(P)$(M)_HOMED") +{ + field(DESC, "Motor Homed") + field(ZNAM, "No") + field(ONAM, "Yes") + field(INP, "$(P)$(M):_MSTABITS.BE CP") +} + #! Further lines contain data used by VisualDCT #! View(405,680,1.0) #! Record("$(P)$(M)",840,674,0,0,"$(P)$(M)") diff --git a/motorApp/Db/pseudoMotor.db b/motorApp/Db/pseudoMotor.db index f961075ed..bcf431b5d 100644 --- a/motorApp/Db/pseudoMotor.db +++ b/motorApp/Db/pseudoMotor.db @@ -42,6 +42,20 @@ grecord(motor,"$(P)$(M)") alias("$(P)$(M)", "$(P)$(M):SP") alias("$(P)$(M)", "$(P)$(M):SP:RBV") +# allow splitting out of MSTA +record(mbbiDirect, "$(P)$(M):_MSTABITS") +{ + field(INP, "$(P)$(M).MSTA CP") +} + +record(bi, "$(P)$(M)_HOMED") +{ + field(DESC, "Motor Homed") + field(ZNAM, "No") + field(ONAM, "Yes") + field(INP, "$(P)$(M):_MSTABITS.BE CP") +} + grecord(transform,"$(P)$(M)_ableput") { field(CLCB,"a") From afc5ab4d97d49e87506c6be95333cf41b80fe3b9 Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Fri, 14 Jan 2022 15:21:31 +0000 Subject: [PATCH 184/232] Update motorUtil.db --- motorApp/Db/motorUtil.db | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index e19d2aa7c..9a9832090 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -82,7 +82,7 @@ $(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") $(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") ## _MOVING2.A is used by reflectometry ioc $(IFIOC_NWPRTXPS_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.B CA") -$(IFIOC_TWINCAT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.C CA") +$(IFIOC_TC_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.C CA") $(IFIOC_GALILMUL_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.D CA") $(IFIOC_GALILMUL_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.E CA") } From dfca9968ca99180eb6c7a5b302c273e3dc7a8d28 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 1 Feb 2022 14:44:11 +0000 Subject: [PATCH 185/232] Fix GALILMUL moving location --- motorApp/Db/motorUtil.db | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 9a9832090..01d050ef8 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -83,8 +83,8 @@ $(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") ## _MOVING2.A is used by reflectometry ioc $(IFIOC_NWPRTXPS_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.B CA") $(IFIOC_TC_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.C CA") -$(IFIOC_GALILMUL_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.D CA") -$(IFIOC_GALILMUL_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING.E CA") +$(IFIOC_GALILMUL_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING2.D CA") +$(IFIOC_GALILMUL_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING2.E CA") } # force periodic processing to catch up any missing, shouldn't From 369aac1c4b5068c722e46f9ae52f31d763d959dc Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 26 Jan 2022 18:01:36 +0000 Subject: [PATCH 186/232] Check more items when deciding when to poll --- motorApp/Db/periodic_polling.db | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/motorApp/Db/periodic_polling.db b/motorApp/Db/periodic_polling.db index f2fa24f4a..55fe55551 100644 --- a/motorApp/Db/periodic_polling.db +++ b/motorApp/Db/periodic_polling.db @@ -1,9 +1,12 @@ -record(ao, "$(P)$(M):SCAN") { - field(SCAN, "$(POLL_RATE=10) second") +record(calcout, "$(P)$(M):SCAN") { field(DTYP, "Soft Channel") field(DESC, "Polls the motor position") + field(INPA, "$(P)$(M).MOVN") + field(INPB, "$(P)$(M).DMOV") + field(INPC, "$(P)$(M).HOMF") + field(INPD, "$(P)$(M).HOMR") + field(CALC, "(A==0)&&(B==1)&&(C==0)&&(D==0)") + field(SCAN, "$(POLL_RATE=10) second") + field(OOPT, "When Non-zero") field(OUT, "$(P)$(M).STUP PP") - field(OVAL, "1") - field(VAL, "1") - field(SDIS, "$(P)$(M).MOVN") } From 01476f30bb5d9db367a53b9c9eddc119c12c1225 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 10 Feb 2022 17:28:27 +0000 Subject: [PATCH 187/232] Check for JOG as well --- motorApp/Db/periodic_polling.db | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/motorApp/Db/periodic_polling.db b/motorApp/Db/periodic_polling.db index 55fe55551..c416160b3 100644 --- a/motorApp/Db/periodic_polling.db +++ b/motorApp/Db/periodic_polling.db @@ -5,7 +5,9 @@ record(calcout, "$(P)$(M):SCAN") { field(INPB, "$(P)$(M).DMOV") field(INPC, "$(P)$(M).HOMF") field(INPD, "$(P)$(M).HOMR") - field(CALC, "(A==0)&&(B==1)&&(C==0)&&(D==0)") + field(INPE, "$(P)$(M).JOGF") + field(INPF, "$(P)$(M).JOGR") + field(CALC, "(A=0)&&(B=1)&&(C=0)&&(D=0)&&(E=0)&&(F=0)") field(SCAN, "$(POLL_RATE=10) second") field(OOPT, "When Non-zero") field(OUT, "$(P)$(M).STUP PP") From 0707e301d5d76783763a9e73fb0c1f83176bcfb6 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 29 Dec 2021 23:25:28 +0000 Subject: [PATCH 188/232] Print out when motor is moving/stopped --- motorApp/MotorSimSrc/motorSimDriver.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index 0188c881f..08fd4dd0a 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -278,7 +278,7 @@ asynStatus motorSimAxis::move(double position, int relative, double minVelocity, { route_pars_t pars; static const char *functionName = "move"; - std::cerr << "motorSimAxis::move axis " << axisNo_ << " position " << position << (relative ? " (relative)" : "(absoute)") << " speed min/max " << minVelocity << "/" << maxVelocity << " acceleration " << acceleration << std::endl; + std::cerr << "motorSimAxis::move axis " << axisNo_ << " position " << position << (relative ? " (relative)" : " (absolute)") << " speed min/max " << minVelocity << "/" << maxVelocity << " acceleration " << acceleration << std::endl; if (relative) position += endpoint_.axis[0].p + enc_offset_; @@ -499,9 +499,15 @@ void motorSimAxis::process(double delta ) } } - lastDone_ = done; double encRatio; pC_->getDoubleParam(axisNo_, pC_->motorEncoderRatio_, &encRatio); + if (!lastDone_ && done) { + std::cerr << "motorSimAxis::process axis " << axisNo_ << " has stopped moving: motor=" << nextpoint_.axis[0].p+enc_offset_ << " encoder=" << (nextpoint_.axis[0].p+enc_offset_) * encRatio << std::endl; + } + if (lastDone_ && !done) { + std::cerr << "motorSimAxis::process axis " << axisNo_ << " has started moving" << std::endl; + } + lastDone_ = done; setDoubleParam (pC_->motorPosition_, (nextpoint_.axis[0].p+enc_offset_)); setDoubleParam (pC_->motorEncoderPosition_, (nextpoint_.axis[0].p+enc_offset_) * encRatio); From 610ac0a43bec43a0b06d1536b81b3e37b26c5b66 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 30 Dec 2021 21:53:37 +0000 Subject: [PATCH 189/232] Print out current position on move --- motorApp/MotorSimSrc/motorSimDriver.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index 08fd4dd0a..fa1cf632e 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -277,8 +277,10 @@ void motorSimController::motorSimTask() asynStatus motorSimAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { route_pars_t pars; + double currentPos; static const char *functionName = "move"; - std::cerr << "motorSimAxis::move axis " << axisNo_ << " position " << position << (relative ? " (relative)" : " (absolute)") << " speed min/max " << minVelocity << "/" << maxVelocity << " acceleration " << acceleration << std::endl; + pC_->getDoubleParam(axisNo_, pC_->motorPosition_, ¤tPos); + std::cerr << "motorSimAxis::move axis " << axisNo_ << " from " << currentPos << "|" << endpoint_.axis[0].p + enc_offset_ << " to " << position << (relative ? " (relative)" : " (absolute)") << " speed min/max " << minVelocity << "/" << maxVelocity << " acceleration " << acceleration << std::endl; if (relative) position += endpoint_.axis[0].p + enc_offset_; @@ -505,7 +507,7 @@ void motorSimAxis::process(double delta ) std::cerr << "motorSimAxis::process axis " << axisNo_ << " has stopped moving: motor=" << nextpoint_.axis[0].p+enc_offset_ << " encoder=" << (nextpoint_.axis[0].p+enc_offset_) * encRatio << std::endl; } if (lastDone_ && !done) { - std::cerr << "motorSimAxis::process axis " << axisNo_ << " has started moving" << std::endl; + std::cerr << "motorSimAxis::process axis " << axisNo_ << " has started moving: motor=" << nextpoint_.axis[0].p+enc_offset_ << " encoder=" << (nextpoint_.axis[0].p+enc_offset_) * encRatio << std::endl; } lastDone_ = done; From ac99c0bbaa5e72893e35d25d8919e6efb418ae57 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sat, 1 Jan 2022 12:44:11 +0000 Subject: [PATCH 190/232] Print out expected move duration --- motorApp/MotorSimSrc/motorSimDriver.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index fa1cf632e..862d290a9 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -280,10 +280,17 @@ asynStatus motorSimAxis::move(double position, int relative, double minVelocity, double currentPos; static const char *functionName = "move"; pC_->getDoubleParam(axisNo_, pC_->motorPosition_, ¤tPos); - std::cerr << "motorSimAxis::move axis " << axisNo_ << " from " << currentPos << "|" << endpoint_.axis[0].p + enc_offset_ << " to " << position << (relative ? " (relative)" : " (absolute)") << " speed min/max " << minVelocity << "/" << maxVelocity << " acceleration " << acceleration << std::endl; + if ( currentPos != (endpoint_.axis[0].p + enc_offset_) ) { // unsure if check is really needed + std::cerr << "motorSimAxis::move axis " << axisNo_ << " unsure of initial position: " << currentPos << " or " << endpoint_.axis[0].p + enc_offset_ << std::endl; + } + std::cerr << "motorSimAxis::move axis " << axisNo_ << " from " << currentPos << " to " << position << (relative ? " (relative)" : " (absolute)") << " speed min/max " << minVelocity << "/" << maxVelocity << " acceleration " << acceleration << std::endl; if (relative) position += endpoint_.axis[0].p + enc_offset_; + if (maxVelocity != 0.0) { + std::cerr << "motorSimAxis::move axis " << axisNo_ << " excluding acceleration/backlash this would take approximately " << fabs((position - currentPos) / maxVelocity) << " seconds" << std::endl; + } + /* 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) ) From dc882423cf5d4e3571d52368e700d4fdbf3ab603 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sun, 2 Jan 2022 19:54:22 +0000 Subject: [PATCH 191/232] Print actual move time --- motorApp/MotorSimSrc/motorSimDriver.cpp | 7 +++++-- motorApp/MotorSimSrc/motorSimDriver.h | 1 + 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index 862d290a9..d00952404 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -71,6 +71,7 @@ motorSimAxis::motorSimAxis(motorSimController *pController, int axis, double low delayedDone_ = 0; lastDone_ = 1; setIntegerParam(pC_->motorStatusHasEncoder_, 1); + epicsTimeGetCurrent(&tStart_); } @@ -314,7 +315,6 @@ asynStatus motorSimAxis::move(double position, int relative, double minVelocity, 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 ); @@ -511,10 +511,13 @@ void motorSimAxis::process(double delta ) double encRatio; pC_->getDoubleParam(axisNo_, pC_->motorEncoderRatio_, &encRatio); if (!lastDone_ && done) { - std::cerr << "motorSimAxis::process axis " << axisNo_ << " has stopped moving: motor=" << nextpoint_.axis[0].p+enc_offset_ << " encoder=" << (nextpoint_.axis[0].p+enc_offset_) * encRatio << std::endl; + epicsTimeStamp tNow; + epicsTimeGetCurrent(&tNow); + std::cerr << "motorSimAxis::process axis " << axisNo_ << " has stopped moving after " << epicsTimeDiffInSeconds(&tNow, &tStart_) << " seconds: motor=" << nextpoint_.axis[0].p+enc_offset_ << " encoder=" << (nextpoint_.axis[0].p+enc_offset_) * encRatio << std::endl; } if (lastDone_ && !done) { std::cerr << "motorSimAxis::process axis " << axisNo_ << " has started moving: motor=" << nextpoint_.axis[0].p+enc_offset_ << " encoder=" << (nextpoint_.axis[0].p+enc_offset_) * encRatio << std::endl; + epicsTimeGetCurrent(&tStart_); } lastDone_ = done; diff --git a/motorApp/MotorSimSrc/motorSimDriver.h b/motorApp/MotorSimSrc/motorSimDriver.h index 78a3e75b5..80d27342c 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.h +++ b/motorApp/MotorSimSrc/motorSimDriver.h @@ -59,6 +59,7 @@ class epicsShareClass motorSimAxis : public asynMotorAxis double lastTimeSecs_; int delayedDone_; int lastDone_; + epicsTimeStamp tStart_; friend class motorSimController; }; From 11d46b11ad5dacff4395852a3d92a4ad9c4ea8fb Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Fri, 22 Apr 2022 16:21:03 +0100 Subject: [PATCH 192/232] remove BKHOFF --- motorApp/Db/motorUtil.db | 1 - 1 file changed, 1 deletion(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 01d050ef8..072b362a4 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -77,7 +77,6 @@ $(IFIOC_MCLEN_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.E CA") $(IFIOC_LINMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.F CA") $(IFIOC_LINMOT_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.G CA") $(IFIOC_LINMOT_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.H CA") -$(IFIOC_BKHOFF_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.I CA") $(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") $(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") ## _MOVING2.A is used by reflectometry ioc From 2345b7a37f16fc0376002f7301134001da784acf Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Fri, 14 Oct 2022 11:50:35 +0100 Subject: [PATCH 193/232] adding _IOCNAME pv to motorstatus.db --- motorApp/Db/motorStatus.db | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/motorApp/Db/motorStatus.db b/motorApp/Db/motorStatus.db index 3ec4f86ad..adb7a681d 100644 --- a/motorApp/Db/motorStatus.db +++ b/motorApp/Db/motorStatus.db @@ -39,3 +39,8 @@ record(ai, "$(P)$(M):DMOV") field(DTYP, "Soft Channel") field(INP, "$(P)$(M).DMOV CP") } + +record(stringin, "$(P)$(M)_IOCNAME){ + field(DESC, "IOC name eg GALIL_01 for mtr rec") + field(VAL, "$(IOCNAME="UNKNOWN")) +} From 71c23e49abc5fe0e2d74b95d2f256e9ee27ef425 Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Fri, 14 Oct 2022 12:53:10 +0100 Subject: [PATCH 194/232] Update motorStatus.db --- motorApp/Db/motorStatus.db | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/Db/motorStatus.db b/motorApp/Db/motorStatus.db index adb7a681d..dabd621ca 100644 --- a/motorApp/Db/motorStatus.db +++ b/motorApp/Db/motorStatus.db @@ -40,7 +40,7 @@ record(ai, "$(P)$(M):DMOV") field(INP, "$(P)$(M).DMOV CP") } -record(stringin, "$(P)$(M)_IOCNAME){ +record(stringin, "$(P)$(M)_IOCNAME"){ field(DESC, "IOC name eg GALIL_01 for mtr rec") field(VAL, "$(IOCNAME="UNKNOWN")) } From 0d84ab7f9954b2ca74516073b78e3e135e504610 Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Fri, 14 Oct 2022 12:53:34 +0100 Subject: [PATCH 195/232] Update motorStatus.db --- motorApp/Db/motorStatus.db | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/Db/motorStatus.db b/motorApp/Db/motorStatus.db index dabd621ca..1993e1552 100644 --- a/motorApp/Db/motorStatus.db +++ b/motorApp/Db/motorStatus.db @@ -42,5 +42,5 @@ record(ai, "$(P)$(M):DMOV") record(stringin, "$(P)$(M)_IOCNAME"){ field(DESC, "IOC name eg GALIL_01 for mtr rec") - field(VAL, "$(IOCNAME="UNKNOWN")) + field(VAL, "$(IOCNAME="UNKNOWN")") } From 84312177406d178b6d619c8b2c18f80c168f9ddf Mon Sep 17 00:00:00 2001 From: Tom Willemsen Date: Tue, 25 Oct 2022 18:10:24 +0100 Subject: [PATCH 196/232] Fix motor moving for huber and galilmul --- motorApp/Db/motorUtil.db | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/motorApp/Db/motorUtil.db b/motorApp/Db/motorUtil.db index 072b362a4..468abf5a5 100644 --- a/motorApp/Db/motorUtil.db +++ b/motorApp/Db/motorUtil.db @@ -78,12 +78,12 @@ $(IFIOC_LINMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.F CA") $(IFIOC_LINMOT_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.G CA") $(IFIOC_LINMOT_03=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.H CA") $(IFIOC_PIMOT_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.J CA") -$(IFIOC_SM300_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") +$(IFIOC_HUBER_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING1.K CA") ## _MOVING2.A is used by reflectometry ioc $(IFIOC_NWPRTXPS_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.B CA") $(IFIOC_TC_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.C CA") -$(IFIOC_GALILMUL_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING2.D CA") -$(IFIOC_GALILMUL_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:MOVING2.E CA") +$(IFIOC_GALILMUL_01=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.D CA") +$(IFIOC_GALILMUL_02=#) field(OUTA, "$(PVPREFIX)CS:MOT:_MOVING2.E CA") } # force periodic processing to catch up any missing, shouldn't From 89e10da343fe7556145ae69a9c0c14619e8b54dc Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 15 Nov 2022 16:38:20 +0000 Subject: [PATCH 197/232] Make homing work whatever FOFF value present --- motorApp/MclennanSrc/homing.st | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index f24026bbb..25a25be97 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -13,15 +13,13 @@ int jog_forward_value, jog_reverse_value; assign jog_forward_value to "{MOTPV}.JOGF"; assign jog_reverse_value to "{MOTPV}.JOGR"; -int set, position, position_d; -double offset; +int set, foff; +double position_d; assign set to "{MOTPV}.SET"; -assign position to "{MOTPV}.VAL"; assign position_d to "{MOTPV}.DVAL"; -assign offset to "{MOTPV}.OFF"; - -double offsetval; +assign foff to "{MOTPV}.FOFF"; +int foff_value; int debug_flag; int debug; int mode; @@ -189,18 +187,15 @@ ss motor errlogSevPrintf(errlogInfo, "Sequencer: axis %i: Setting position to zero (modes 2 and 3)\n", axis); } - pvGet(offset); - offsetval = offset; - + pvGet(foff); + foff_value = foff; + PVPUT(foff, 1); /* frozen offset for applying home position */ PVPUT(set, 1); - PVPUT(position, 0.0); PVPUT(position_d, 0.0); - /* Reapply the offset */ - PVPUT(offset, offsetval); - PVPUT(set, 0); + PVPUT(foff, foff_value); /* restore previous offset mode */ } if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM moving TO done\n", axis); From 9b79fb9916a116b36273b7f7477d43cc634cdda2 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 15 Nov 2022 16:41:26 +0000 Subject: [PATCH 198/232] tab -> space --- motorApp/MclennanSrc/homing.st | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 25a25be97..26ce1f82c 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -188,7 +188,7 @@ ss motor } pvGet(foff); - foff_value = foff; + foff_value = foff; PVPUT(foff, 1); /* frozen offset for applying home position */ PVPUT(set, 1); From d33d0fe6ec6ab0669cb6b598ac6c287358daf2bd Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 17 Apr 2023 01:20:14 +0100 Subject: [PATCH 199/232] Fix various issues - fix issue with snl also triggering datum hardware homing - add many more diagnostics and bulting homing happening at same time --- motorApp/MclennanSrc/devPM304.cc | 58 +++++++++++++++++------ motorApp/MclennanSrc/drvPM304.cc | 80 ++++++++++++++++++++++++++------ motorApp/MclennanSrc/drvPM304.h | 2 + 3 files changed, 113 insertions(+), 27 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index bb88d655a..14d201a77 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -175,17 +175,13 @@ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int strcat(motor_call->message, buff); sprintf(buff, "%dIX%d;", axis, home_direction); } else { - if ( home_mode==HOME_MODE_BUILTIN || home_mode==HOME_MODE_REVERSE_HOME_AND_ZERO || home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO) { - if ( home_mode==HOME_MODE_REVERSE_HOME_AND_ZERO ) { - home_direction = -1; - } else if ( home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO ) { - home_direction = 1; - } + if ( home_mode == HOME_MODE_BUILTIN ) { sprintf(buff, "%dSC%d;", axis, creep_speed); strcat(motor_call->message, buff); sprintf(buff, "%dHD%d;", axis, home_direction); + // home position may or may not be automatically applied at end of HD depending on datum mode setting } else { - // Let SNL take care of everything, so do not reset creep speed. See homing.st + // For all other home modes let SNL take care of everything, so do not reset creep speed. See homing.st } } strcat(motor_call->message, buff); @@ -270,7 +266,9 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo request_home(motor_call, cntrl->model, axis, 1, cntrl->home_mode[axis-1]); break; case LOAD_POS: - if (cntrl->use_encoder[axis-1]){ + sprintf(buff, "%dCP%ld;", axis, ival); /* check if need to scale by encoder ratio, think not ? */ + if (cntrl->use_encoder[axis-1] || cntrl->model != MODEL_PM304){ + strcat(motor_call->message, buff); sprintf(buff, "%dAP%ld;", axis, ival); } break; @@ -331,15 +329,31 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo } break; case SET_PGAIN: - sprintf(buff, "%dKP%ld;", axis, ival); + // for servo mode we need NINT(dval * 32767.0) + // in stepper mode this is used for end of move position correction + if (cntrl->model == MODEL_PM304) { + sprintf(buff, "%dKP%ld;", axis, ival); + } else if (cntrl->control_mode[axis-1] == 1) { + sprintf(buff, "%dKP%ld;", axis, NINT(dval * 32767.0)); /* servo */ + } else { + sprintf(buff, "%dKP%ld;", axis, NINT(dval * 100.0)); /* stepper */ + } break; case SET_IGAIN: - sprintf(buff, "%dKS%ld;", axis, ival); + if (cntrl->model == MODEL_PM304) { + sprintf(buff, "%dKS%ld;", axis, ival); + } else if (cntrl->control_mode[axis-1] == 1) { + sprintf(buff, "%dKS%ld;", axis, NINT(dval * 32767.0)); // only valid in servo mode on PM600 + } break; case SET_DGAIN: - sprintf(buff, "%dKV%ld;", axis, ival); + if (cntrl->model == MODEL_PM304) { + sprintf(buff, "%dKV%ld;", axis, ival); + } else if (cntrl->control_mode[axis-1] == 1) { + sprintf(buff, "%dKV%ld;", axis, NINT(dval * 32767.0)); // only valid in servo mode on PM600 + } break; case ENABLE_TORQUE: @@ -350,11 +364,27 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dAB;", axis); break; + /* limits may or may not be enforced depending on last SL command + hardware will not let you set a low limit >= high limit so order of setting + may be important. Need to look more closely at motor record */ case SET_HIGH_LIMIT: + if (false) { + sprintf(buff, "%dUL%ld;", axis, ival); + } else { + trans->state = IDLE_STATE; /* No command sent to the controller. */ + /* The PM304 internal soft limits are very difficult to retrieve, not + * implemented yet */ + } + break; + case SET_LOW_LIMIT: - trans->state = IDLE_STATE; /* No command sent to the controller. */ - /* The PM304 internal soft limits are very difficult to retrieve, not - * implemented yet */ + if (false) { + sprintf(buff, "%dLL%ld;", axis, ival); + } else { + trans->state = IDLE_STATE; /* No command sent to the controller. */ + /* The PM304 internal soft limits are very difficult to retrieve, not + * implemented yet */ + } break; default: diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index ab14c4401..7074ce87d 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -150,21 +150,44 @@ STATIC struct thread_args targs = {SCAN_RATE, &PM304_access, 0.0}; *********************************************************/ static long report(int level) { - int card; + int card, motor_index; + struct PM304controller *cntrl; + char command[BUFF_SIZE]; + char buff[1024]; /* need bigger than usual as QA can return a lot */ if (PM304_num_cards <=0) - printf(" NO PM304 controllers found\n"); + printf(" NO PM304/PM600 controllers found\n"); else { for (card = 0; card < PM304_num_cards; card++) - if (motor_state[card]) - printf(" PM304 controller %d, id: %s \n", + if (motor_state[card]) { + cntrl = (struct PM304controller *) motor_state[card]->DevicePrivate; + printf(" %s controller %d, id: %s \n num axes %d\n", + (cntrl->model == MODEL_PM304 ? "PM304" : "PM600"), card, - motor_state[card]->ident); + motor_state[card]->ident, + cntrl->n_axes); + for(motor_index = 0; motor_index < cntrl->n_axes; motor_index++) { + sprintf(command, "%dQA", motor_index+1); + send_recv_mess(card, command, buff); + printf("%s\n", buff); + sprintf(command, "%dQM", motor_index+1); + send_recv_mess(card, command, buff); + printf("%s\n", buff); + sprintf(command, "%dQS", motor_index+1); + send_recv_mess(card, command, buff); + printf("%s\n", buff); + sprintf(command, "%dQP", motor_index+1); + send_recv_mess(card, command, buff); + printf("%s\n", buff); + } + } } return (0); } + + static long init() { @@ -278,15 +301,29 @@ STATIC int set_status(int card, int signal) } status.Bits.RA_PROBLEM = (response[1] == '1') ? 1 : 0; - + if (response[2] == '1') { - status.Bits.RA_PLUS_LS = 1; + status.Bits.RA_PLUS_LS = 1; /* need to set ls_active = true; ? */ } if (response[3] == '1') { - status.Bits.RA_MINUS_LS = 1; + status.Bits.RA_MINUS_LS = 1; /* need to set ls_active = true; ? */ } } + if (cntrl->model != MODEL_PM304) { + char *op; + sprintf(command, "%dCO", signal+1); + send_recv_mess(card, command, response); + Debug(2, "set_status, operation query, card %d, response=%s\n", card, response); + /* returns Mode = XXX */ + op = strchr(response, '='); + if (op != NULL) { + if (strncmp(cntrl->current_op[signal], op + 1, sizeof(cntrl->current_op[0]))) { + Debug(1, "set_status: card %d axis %d: %s\n", card, signal + 1, op + 1); + strncpy(cntrl->current_op[signal], op + 1, sizeof(cntrl->current_op[0])); + } + } + } /* encoder status */ status.Bits.EA_SLIP = 0; @@ -398,6 +435,9 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) level = 2; controller_error = 0; } + if (strchr(response, '!')) { /* an error contains an ! */ + level = 1; + } Debug(level, "send_mess: card %d, response=...\n%s\n", card, response); } @@ -420,6 +460,7 @@ STATIC int recv_mess(int card, char *com, int flag) char *pos; char temp[BUFF_SIZE]; int flush; + int level = 2; asynStatus status; size_t nread=0; int eomReason; @@ -449,7 +490,10 @@ STATIC int recv_mess(int card, char *com, int flag) /* The response from the PM304 is terminated with CR/LF. Remove these */ if (nread == 0) com[0] = '\0'; if (nread > 0) { - Debug(2, "recv_mess: card %d, flag=%d, message = \"%s\"\n", card, flag, com); + if (strchr(com, '!')) { /* errors contain ! */ + level = 1; + } + Debug(level, "recv_mess: card %d, flag=%d, message = \"%s\"\n", card, flag, com); } if (nread == 0) { if (flag != FLUSH) { @@ -492,6 +536,7 @@ STATIC int send_recv_mess(int card, const char *out, char *response) size_t nwrite=0, nread=0; int eomReason; char temp[BUFF_SIZE]; + int level = 2; response[0] = '\0'; @@ -518,9 +563,12 @@ STATIC int send_recv_mess(int card, const char *out, char *response) } /* The response from the PM304 is terminated with CR/LF. Remove these */ - if (nread == 0) response[0] = '\0';; + if (nread == 0) response[0] = '\0'; + if (strchr(response, '!')) { + level = 1; + } if (nread > 0) { - Debug(2, "send_recv_mess: card %d, response = \"%s\"\n", card, response); + Debug(level, "send_recv_mess: card %d, response = \"%s\"\n", card, response); } if (nread == 0) { Debug(1, "send_recv_mess: card %d ERROR: no response\n", card); @@ -595,7 +643,7 @@ PM304Config(int card, /* card being configured */ if (n_axes == 0) n_axes=1; /* This is a new parameter, some startup files don't have it yet */ motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); - motor_state[card]->DevicePrivate = malloc(sizeof(struct PM304controller)); + motor_state[card]->DevicePrivate = calloc(1, sizeof(struct PM304controller)); cntrl = (struct PM304controller *) motor_state[card]->DevicePrivate; cntrl->n_axes = n_axes; for (int i=0; iuse_encoder[motor_index] = 1; } } - + if (cntrl->model == MODEL_PM600) { + sprintf(command, "%dQM", motor_index+1); + send_recv_mess(card_index, command, buff); /* 01:CM = 1 AM = 00000000 DM = 00010000 JM = 11000000 */ + if (strchr(buff, '=') != NULL) { + cntrl->control_mode[motor_index] = atoi(strchr(buff, '=') + 1); + } + } /* Querying speeds for this axis */ sprintf(command, "%dQS", motor_index+1); send_recv_mess(card_index, command, buff); diff --git a/motorApp/MclennanSrc/drvPM304.h b/motorApp/MclennanSrc/drvPM304.h index eb07226a6..50439db14 100644 --- a/motorApp/MclennanSrc/drvPM304.h +++ b/motorApp/MclennanSrc/drvPM304.h @@ -40,6 +40,8 @@ struct PM304controller char port[80]; /* Asyn port name */ int reset_before_move; /* Reset the controller before any move command */ int creep_speeds[PM304_MAX_CHANNELS]; /* Creep speed for each axis */ + char current_op[PM304_MAX_CHANNELS][60]; /* PM600: current operation as per CO command */ + int control_mode[PM304_MAX_CHANNELS]; /* PM600: comtrol mode as per CM command */ }; RTN_STATUS PM304Setup(int, int); From b2d848cafc35b0efe4e5e343d604372cba524a1c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 18 Apr 2023 01:37:31 +0100 Subject: [PATCH 200/232] Stop running on builtin homing --- motorApp/MclennanSrc/homing.st | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 26ce1f82c..91857b62c 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -74,7 +74,7 @@ ss motor state ready { - when (home_reverse_pv==1) + when (home_reverse_pv==1 && mode != HOME_MODE_BUILTIN) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO reverse_home_requested\n", axis); @@ -89,7 +89,7 @@ ss motor } } state reverse_home_requested - when ((home_forward_pv==1) && (mode!=HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) + when ((home_forward_pv==1) && (mode!=HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO) && (mode != HOME_MODE_BUILTIN)) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO forward_home_requested\n", axis); From 974726c25ae9674f870579cf08a983ffda4b4a48 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 18 Apr 2023 01:38:18 +0100 Subject: [PATCH 201/232] Fix some buffer initialisation issues --- motorApp/MclennanSrc/devPM304.cc | 4 +++- motorApp/MclennanSrc/drvPM304.cc | 20 +++++++++++--------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 14d201a77..22c9dd848 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -49,7 +49,7 @@ extern "C" {epicsExportAddress(int, devPM304Debug);} static inline void Debug(int level, const char *format, ...) { #ifdef DEBUG - if (level < devPM304Debug) { + if (level <= devPM304Debug) { va_list pVar; va_start(pVar, format); vprintf(format, pVar); @@ -170,6 +170,7 @@ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int // Max creep speed is 800 - set to velo if under 800 int creep_speed = (VELO>800) ? 800 : VELO; char buff[30]; + buff[0] = '\0'; if (model == MODEL_PM304){ sprintf(buff, "%dSC%d;", axis, creep_speed); strcat(motor_call->message, buff); @@ -245,6 +246,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo if (cntrl->reset_before_move==1) { sprintf(buff, "%dRS;", axis); strcat(motor_call->message, buff); + buff[0] = '\0'; } break; default: diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 7074ce87d..2a0927d27 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -67,7 +67,7 @@ extern "C" {epicsExportAddress(int, drvPM304Debug);} static inline void Debug(int level, const char *format, ...) { #ifdef DEBUG - if (level < drvPM304Debug) { + if (level <= drvPM304Debug) { va_list pVar; va_start(pVar, format); vprintf(format, pVar); @@ -427,16 +427,18 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); /* Set the debug level for most responses to be 2. Flag reset messages * to 1 so we can spot them more easily */ - int level; - if (strcmp(&p[1],"RS")==0 && strstr(response, "NOT ABORTED") == NULL) { - level = 1; - controller_error = 1; - } else { - level = 2; - controller_error = 0; - } + int level = 2; if (strchr(response, '!')) { /* an error contains an ! */ level = 1; + } + if (strcmp(&p[1],"RS")==0) { + if (strstr(response, "NOT ABORTED") == NULL && strstr(response, "RESET") == NULL) { // actual string is !NOT ABORTED + level = 1; + controller_error = 1; + } else { + level = 2; + controller_error = 0; + } } Debug(level, "send_mess: card %d, response=...\n%s\n", card, response); } From c0548d7b2608d055d4b6128a4840f3510910f406 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 18 Apr 2023 21:31:38 +0100 Subject: [PATCH 202/232] Tidy up comments --- motorApp/MclennanSrc/drvPM304.cc | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 2a0927d27..9c427dc4a 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -421,25 +421,31 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) for (p = epicsStrtok_r(temp, ";", &tok_save); ((p != NULL) && (strlen(p) != 0)); p = epicsStrtok_r(NULL, ";", &tok_save)) { - + Debug(2, "send_mess: sending message to card %d, message=%s\n", card, p); - pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, + pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); - /* Set the debug level for most responses to be 2. Flag reset messages - * to 1 so we can spot them more easily */ - int level = 2; + /* Set the debug level for most responses to be 2. Flag reset messages + * to 1 so we can spot them more easily */ + int level = 2; if (strchr(response, '!')) { /* an error contains an ! */ level = 1; } - if (strcmp(&p[1],"RS")==0) { + if (strcmp(&p[1],"RS")==0) { if (strstr(response, "NOT ABORTED") == NULL && strstr(response, "RESET") == NULL) { // actual string is !NOT ABORTED - level = 1; - controller_error = 1; + level = 1; + controller_error = 1; } else { - level = 2; - controller_error = 0; + level = 2; + controller_error = 0; } + } else { + // this is to maintain previous behaviour, but should we always clear? Because this loop splits + // commands a reset before move will not see a controller_error as it will get cleared when the move + // command is executed. So only a reset on its own that fails will show. Is that intended? + controller_error = 0; } + Debug(level, "send_mess: card %d, response=...\n%s\n", card, response); } From ac58b1d4082b83124914d870a3f5aa58621da874 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 20 Apr 2023 21:59:20 +0100 Subject: [PATCH 203/232] More diagnostics --- motorApp/MclennanSrc/devPM304.cc | 22 +++++++++-- motorApp/MclennanSrc/drvPM304.cc | 67 +++++++++++++++++++++++++------- motorApp/MclennanSrc/drvPM304.h | 4 +- motorApp/MclennanSrc/homing.st | 4 +- 4 files changed, 75 insertions(+), 22 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 22c9dd848..25eac222c 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -166,7 +166,7 @@ STATIC RTN_STATUS PM304_end_trans(struct motorRecord *mr) } /* request homing move */ -STATIC void request_home(struct mess_node *motor_call, int model, int axis, int home_direction, int home_mode) { +STATIC void request_home(struct mess_node *motor_call, int model, int axis, int home_direction, int home_mode, char* datum_mode) { // Max creep speed is 800 - set to velo if under 800 int creep_speed = (VELO>800) ? 800 : VELO; char buff[30]; @@ -176,7 +176,21 @@ STATIC void request_home(struct mess_node *motor_call, int model, int axis, int strcat(motor_call->message, buff); sprintf(buff, "%dIX%d;", axis, home_direction); } else { - if ( home_mode == HOME_MODE_BUILTIN ) { + // datum mode: [0] = encoder index input polarity, [3] automatic direction search, [4] automatic opposite limit search + if ( home_mode==HOME_MODE_BUILTIN || home_mode==HOME_MODE_REVERSE_HOME_AND_ZERO || home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO) { + datum_mode[1] = '0'; // set datum mode to capture only on HD command + if ( home_mode==HOME_MODE_REVERSE_HOME_AND_ZERO ) { + sprintf(buff, "%dSH0;", axis); // define home position as 0 + datum_mode[2] = '1'; // set datum mode to apply home position + home_direction = -1; + } else if ( home_mode==HOME_MODE_FORWARD_HOME_AND_ZERO ) { + sprintf(buff, "%dSH0;", axis); // define home position as 0 + datum_mode[2] = '1'; // set datum mode to apply home position + home_direction = 1; + } + strcat(motor_call->message, buff); + sprintf(buff, "%dDM%s;", axis, datum_mode); + strcat(motor_call->message, buff); sprintf(buff, "%dSC%d;", axis, creep_speed); strcat(motor_call->message, buff); sprintf(buff, "%dHD%d;", axis, home_direction); @@ -262,10 +276,10 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dMR%ld;", axis, ival); break; case HOME_REV: - request_home(motor_call, cntrl->model, axis, -1, cntrl->home_mode[axis-1]); + request_home(motor_call, cntrl->model, axis, -1, cntrl->home_mode[axis-1], cntrl->datum_mode[axis-1]); break; case HOME_FOR: - request_home(motor_call, cntrl->model, axis, 1, cntrl->home_mode[axis-1]); + request_home(motor_call, cntrl->model, axis, 1, cntrl->home_mode[axis-1], cntrl->datum_mode[axis-1]); break; case LOAD_POS: sprintf(buff, "%dCP%ld;", axis, ival); /* check if need to scale by encoder ratio, think not ? */ diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 9c427dc4a..b26c556ad 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -128,6 +128,27 @@ struct driver_table PM304_access = NULL }; +void printDatumMode(const char* mode) +{ + printf("Datum Mode (DM) is %s\n", mode); + printf(" Encoder index input polarity is %s\n", (mode[0] == '0' ? "normal" : "inverted")); + printf(" Datum point is captured %s\n", (mode[1] == '0' ? "only once (i.e. after HD command) " : "each time it happens")); + printf(" Datum position is %s\n", (mode[2] == '0' ? "captured but not changed" : "set to Home Position (SH) after datum search (HD)")); + printf(" Automatic direction search %s\n", (mode[3] == '0' ? "disabled" : "enabled")); + printf(" Automatic opposite limit search %s\n", (mode[4] == '0' ? "disabled" : "enabled")); +} + +void printAbortMode(const char* mode) +{ + printf("Abort Mode (AM) is %s\n", mode); + printf(" Abort Stop Input %s\n", (mode[0] == '0' ? "disables control loop" : "stops all moves only")); + printf(" Abort Stop Input is %s\n", (mode[1] == '0' ? "latched requiring RS command to reset" : "only momentary")); + printf(" Stall Error %s\n", (mode[2] == '0' ? "disables control loop" : "is indicated but control loop remains active")); + printf(" Tracking Error %s\n", (mode[3] == '0' ? "disables control loop" : "is indicated but control loop remains active")); + printf(" TimeOut Error %s\n", (mode[4] == '0' ? "disables control loop" : "is indicated but control loop remains active")); + printf(" Enable output %s\n", (mode[7] == '0' ? "switched OFF during a disabled control loop" : "left ON during a control loop abort")); +} + struct drvPM304_drvet { long number; @@ -168,26 +189,33 @@ static long report(int level) motor_state[card]->ident, cntrl->n_axes); for(motor_index = 0; motor_index < cntrl->n_axes; motor_index++) { - sprintf(command, "%dQA", motor_index+1); - send_recv_mess(card, command, buff); - printf("%s\n", buff); - sprintf(command, "%dQM", motor_index+1); - send_recv_mess(card, command, buff); - printf("%s\n", buff); - sprintf(command, "%dQS", motor_index+1); - send_recv_mess(card, command, buff); - printf("%s\n", buff); - sprintf(command, "%dQP", motor_index+1); - send_recv_mess(card, command, buff); - printf("%s\n", buff); + printf("** Axis %d **\n", motor_index+1); + printf(" reset before move %s\n", (cntrl->reset_before_move ? "YES" : "NO")); + printf(" creep speed %d\n", cntrl->creep_speeds[motor_index]); + printf(" use encoder %s\n", (cntrl->use_encoder[motor_index] ? "YES" : "NO")); + printf(" home mode %d\n", cntrl->home_mode[motor_index]); + printf(" control mode %d\n", cntrl->control_mode[motor_index]); + printf(" current op %s\n", cntrl->current_op[motor_index]); + printDatumMode(cntrl->datum_mode[motor_index]); + printAbortMode(cntrl->abort_mode[motor_index]); + sprintf(command, "%dQM", motor_index+1); + send_recv_mess(card, command, buff); + printf("%s\n", buff); + sprintf(command, "%dQS", motor_index+1); + send_recv_mess(card, command, buff); + printf("%s\n", buff); + sprintf(command, "%dQP", motor_index+1); + send_recv_mess(card, command, buff); + printf("%s\n", buff); + sprintf(command, "%dQA", motor_index+1); + send_recv_mess(card, command, buff); + printf("%s\n", buff); } } } return (0); } - - static long init() { @@ -308,6 +336,7 @@ STATIC int set_status(int card, int signal) if (response[3] == '1') { status.Bits.RA_MINUS_LS = 1; /* need to set ls_active = true; ? */ } + status.Bits.RA_HOME = (response[5] == '1') ? 1 : 0; } if (cntrl->model != MODEL_PM304) { @@ -775,12 +804,20 @@ STATIC int motor_init() cntrl->use_encoder[motor_index] = 1; } } - if (cntrl->model == MODEL_PM600) { + if (cntrl->model != MODEL_PM304) { sprintf(command, "%dQM", motor_index+1); send_recv_mess(card_index, command, buff); /* 01:CM = 1 AM = 00000000 DM = 00010000 JM = 11000000 */ if (strchr(buff, '=') != NULL) { cntrl->control_mode[motor_index] = atoi(strchr(buff, '=') + 1); } + if (strstr(buff, "DM =") != NULL) { + int dm = atoi(strstr(buff, "DM =") + 1); + epicsSnprintf(cntrl->datum_mode[motor_index], sizeof(cntrl->datum_mode[motor_index]), "%08d", dm); + } + if (strstr(buff, "AM =") != NULL) { + int am = atoi(strstr(buff, "AM =") + 1); + epicsSnprintf(cntrl->abort_mode[motor_index], sizeof(cntrl->abort_mode[motor_index]), "%08d", am); + } } /* Querying speeds for this axis */ sprintf(command, "%dQS", motor_index+1); diff --git a/motorApp/MclennanSrc/drvPM304.h b/motorApp/MclennanSrc/drvPM304.h index 50439db14..0262423bb 100644 --- a/motorApp/MclennanSrc/drvPM304.h +++ b/motorApp/MclennanSrc/drvPM304.h @@ -41,7 +41,9 @@ struct PM304controller int reset_before_move; /* Reset the controller before any move command */ int creep_speeds[PM304_MAX_CHANNELS]; /* Creep speed for each axis */ char current_op[PM304_MAX_CHANNELS][60]; /* PM600: current operation as per CO command */ - int control_mode[PM304_MAX_CHANNELS]; /* PM600: comtrol mode as per CM command */ + int control_mode[PM304_MAX_CHANNELS]; /* PM600: datum mode as per CM command */ + char datum_mode[PM304_MAX_CHANNELS][9]; /* PM600: datum mode as per DM command */ + char abort_mode[PM304_MAX_CHANNELS][9]; /* PM600: abort mode as per AM command */ }; RTN_STATUS PM304Setup(int, int); diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st index 91857b62c..8a384431b 100644 --- a/motorApp/MclennanSrc/homing.st +++ b/motorApp/MclennanSrc/homing.st @@ -74,7 +74,7 @@ ss motor state ready { - when (home_reverse_pv==1 && mode != HOME_MODE_BUILTIN) + when (home_reverse_pv==1 && (mode==HOME_MODE_CONST_VELOCITY_MOVE||mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO reverse_home_requested\n", axis); @@ -89,7 +89,7 @@ ss motor } } state reverse_home_requested - when ((home_forward_pv==1) && (mode!=HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO) && (mode != HOME_MODE_BUILTIN)) + when ((home_forward_pv==1) && (mode==HOME_MODE_CONST_VELOCITY_MOVE)) { if (debug) { errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO forward_home_requested\n", axis); From cd193b4f64fe93e89ac8180af7c4190b8e723a0c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 21 Apr 2023 13:25:36 +0100 Subject: [PATCH 204/232] Fix mode reply syntax from hardware --- motorApp/MclennanSrc/drvPM304.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index b26c556ad..691598ed6 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -344,8 +344,8 @@ STATIC int set_status(int card, int signal) sprintf(command, "%dCO", signal+1); send_recv_mess(card, command, response); Debug(2, "set_status, operation query, card %d, response=%s\n", card, response); - /* returns Mode = XXX */ - op = strchr(response, '='); + /* returns 01:XXX */ + op = strchr(response, ':'); if (op != NULL) { if (strncmp(cntrl->current_op[signal], op + 1, sizeof(cntrl->current_op[0]))) { Debug(1, "set_status: card %d axis %d: %s\n", card, signal + 1, op + 1); @@ -475,7 +475,7 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) controller_error = 0; } - Debug(level, "send_mess: card %d, response=...\n%s\n", card, response); + Debug(level, "send_mess: card %d, message=%s response=...\n%s\n", card, p, response); } return(OK); From 61d92939ec4987463ad523db6bebedd17c964000 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 21 Apr 2023 18:36:31 +0100 Subject: [PATCH 205/232] Ignore error from ST command --- motorApp/MclennanSrc/drvPM304.cc | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 691598ed6..02c4d1ded 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -317,6 +317,7 @@ STATIC int set_status(int card, int signal) status.Bits.RA_DIRECTION = 0; ls_active = true; } + status.Bits.EA_HOME = 0; } else { /* The response string is 01: followed by an eight character string of ones and zeroes */ strcpy(response, &response[3]); @@ -336,7 +337,9 @@ STATIC int set_status(int card, int signal) if (response[3] == '1') { status.Bits.RA_MINUS_LS = 1; /* need to set ls_active = true; ? */ } - status.Bits.RA_HOME = (response[5] == '1') ? 1 : 0; + // [5] seems to be on most of the time +// status.Bits.RA_HOME = (response[5] == '1') ? 1 : 0; +// status.Bits.EA_HOME = (response[5] == '1') ? 1 : 0; } if (cntrl->model != MODEL_PM304) { @@ -355,10 +358,10 @@ STATIC int set_status(int card, int signal) } /* encoder status */ + status.Bits.EA_HOME = 0; status.Bits.EA_SLIP = 0; status.Bits.EA_POSITION = 0; status.Bits.EA_SLIP_STALL = 0; - status.Bits.EA_HOME = 0; /* Request the position of this motor */ if (cntrl->use_encoder[signal]) { @@ -474,7 +477,12 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) // command is executed. So only a reset on its own that fails will show. Is that intended? controller_error = 0; } - + if (strcmp(&p[1], "ST") == 0) { + // ignore error expected if device is not moving + if (strstr(response, "NOT ALLOWED IN THIS MODE") != NULL) { + level = 2; + } + } Debug(level, "send_mess: card %d, message=%s response=...\n%s\n", card, p, response); } From 94b1ee160b40c2cd1e7eae39e44b65964bed3c2a Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 21 Apr 2023 23:14:00 +0100 Subject: [PATCH 206/232] Tidy up loop --- motorApp/MclennanSrc/drvPM304.cc | 100 ++++++++++++++++--------------- 1 file changed, 53 insertions(+), 47 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 02c4d1ded..126ebda26 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -207,9 +207,10 @@ static long report(int level) sprintf(command, "%dQP", motor_index+1); send_recv_mess(card, command, buff); printf("%s\n", buff); - sprintf(command, "%dQA", motor_index+1); - send_recv_mess(card, command, buff); - printf("%s\n", buff); + //disable QA for moment + //sprintf(command, "%dQA", motor_index+1); + //send_recv_mess(card, command, buff); + //printf("%s\n", buff); } } } @@ -430,7 +431,7 @@ STATIC int set_status(int card, int signal) /*****************************************************/ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) { - char *p, *tok_save; + char *p, *tok_save = NULL; char response[BUFF_SIZE]; char temp[BUFF_SIZE]; struct PM304controller *cntrl; @@ -455,11 +456,17 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) p = epicsStrtok_r(NULL, ";", &tok_save)) { Debug(2, "send_mess: sending message to card %d, message=%s\n", card, p); + nread = 0; + response[0] = '\0'; pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), response, BUFF_SIZE, TIMEOUT, &nwrite, &nread, &eomReason); /* Set the debug level for most responses to be 2. Flag reset messages * to 1 so we can spot them more easily */ int level = 2; + if (nread == 0) { + response[0] = '\0'; + Debug(1, "send_mess: card %d message=%s read ERROR: no response\n", card, p); + } if (strchr(response, '!')) { /* an error contains an ! */ level = 1; } @@ -574,7 +581,7 @@ STATIC int recv_mess(int card, char *com, int flag) /*****************************************************/ STATIC int send_recv_mess(int card, const char *out, char *response) { - char *p, *tok_save; + char *p, *tok_save = NULL; struct PM304controller *cntrl; char *pos; asynStatus status; @@ -583,8 +590,6 @@ STATIC int send_recv_mess(int card, const char *out, char *response) char temp[BUFF_SIZE]; int level = 2; - response[0] = '\0'; - /* Check that card exists */ if (!motor_state[card]) { @@ -599,34 +604,36 @@ STATIC int send_recv_mess(int card, const char *out, char *response) * so send them separately */ strcpy(temp, out); for (p = epicsStrtok_r(temp, ";", &tok_save); - ((p != NULL) && (strlen(p) != 0)); - p = epicsStrtok_r(NULL, ";", &tok_save)) { + ((p != NULL) && (strlen(p) != 0)); + p = epicsStrtok_r(NULL, ";", &tok_save)) { + nread = 0; + response[0] = '\0'; Debug(2, "send_recv_mess: sending message to card %d, message=%s\n", card, p); - status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), - response, BUFF_SIZE, TIMEOUT, - &nwrite, &nread, &eomReason); - } + status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), + response, BUFF_SIZE, TIMEOUT, + &nwrite, &nread, &eomReason); - /* The response from the PM304 is terminated with CR/LF. Remove these */ - if (nread == 0) response[0] = '\0'; - if (strchr(response, '!')) { - level = 1; - } - if (nread > 0) { - Debug(level, "send_recv_mess: card %d, response = \"%s\"\n", card, response); - } - if (nread == 0) { - Debug(1, "send_recv_mess: card %d ERROR: no response\n", card); - } - /* The PM600 always echoes the command sent to it, before sending the response. It is terminated - with a carriage return. So we need to delete all characters up to and including the first - carriage return */ - if (cntrl->model == MODEL_PM600) { - pos = strchr(response, '\r'); - if (pos != NULL) { - strcpy(temp, pos+1); - strcpy(response, temp); - } + /* The response from the PM304 is terminated with CR/LF. Remove these */ + if (nread == 0) response[0] = '\0'; + if (strchr(response, '!')) { + level = 1; + } + if (nread > 0) { + Debug(level, "send_recv_mess: card %d, message = %s, response = \"%s\"\n", card, p, response); + } + if (nread == 0) { + Debug(1, "send_recv_mess: card %d, message = %s, ERROR: no response\n", card, p); + } + /* The PM600 always echoes the command sent to it, before sending the response. It is terminated + with a carriage return. So we need to delete all characters up to and including the first + carriage return */ + if (cntrl->model == MODEL_PM600) { + pos = strchr(response, '\r'); + if (pos != NULL) { + strcpy(temp, pos + 1); + strcpy(response, temp); + } + } } return(strlen(response)); } @@ -798,20 +805,6 @@ STATIC int motor_init() motor_info->no_motion_count = 0; motor_info->encoder_position = 0; motor_info->position = 0; - /* Figure out if we have an encoder or not. If so use 0A, else use OC for readback. */ - sprintf(command, "%dID", motor_index+1); - send_recv_mess(card_index, command, buff); /* Read controller ID string for this axis */ - if (cntrl->model == MODEL_PM304) { - /* For now always assume encoder if PM304 - needs work */ - cntrl->use_encoder[motor_index] = 1; - } else { - /* PM600 ident string identifies open loop stepper motors */ - if (strstr(buff, "Open loop stepper mode") != NULL) { - cntrl->use_encoder[motor_index] = 0; - } else { - cntrl->use_encoder[motor_index] = 1; - } - } if (cntrl->model != MODEL_PM304) { sprintf(command, "%dQM", motor_index+1); send_recv_mess(card_index, command, buff); /* 01:CM = 1 AM = 00000000 DM = 00010000 JM = 11000000 */ @@ -827,6 +820,19 @@ STATIC int motor_init() epicsSnprintf(cntrl->abort_mode[motor_index], sizeof(cntrl->abort_mode[motor_index]), "%08d", am); } } + /* Figure out if we have an encoder or not. If so use 0A, else use OC for readback. */ + sprintf(command, "%dID", motor_index+1); + send_recv_mess(card_index, command, buff); /* Read controller ID string for this axis */ + if (cntrl->model == MODEL_PM304) { + /* For now always assume encoder if PM304 - needs work */ + cntrl->use_encoder[motor_index] = 1; + } else { + if (cntrl->control_mode[motor_index] == 11) { // CM11 Open loop stepper mode + cntrl->use_encoder[motor_index] = 0; + } else { + cntrl->use_encoder[motor_index] = 1; + } + } /* Querying speeds for this axis */ sprintf(command, "%dQS", motor_index+1); send_recv_mess(card_index, command, buff); From e114b551aa6b6fc1ad1f55837da07f4d0cf159c4 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 12 May 2023 14:35:27 +0100 Subject: [PATCH 207/232] Support QA, better velocity setting --- motorApp/MclennanSrc/devPM304.cc | 66 +++++++++++++++++--------------- motorApp/MclennanSrc/drvPM304.cc | 47 ++++++++++++----------- motorApp/MclennanSrc/drvPM304.h | 1 + 3 files changed, 61 insertions(+), 53 deletions(-) diff --git a/motorApp/MclennanSrc/devPM304.cc b/motorApp/MclennanSrc/devPM304.cc index 25eac222c..ba66bd0b1 100644 --- a/motorApp/MclennanSrc/devPM304.cc +++ b/motorApp/MclennanSrc/devPM304.cc @@ -72,7 +72,6 @@ STATIC long PM304_init_record(void *); STATIC long PM304_start_trans(struct motorRecord *); STATIC RTN_STATUS PM304_build_trans(motor_cmnd, double *, struct motorRecord *); STATIC RTN_STATUS PM304_end_trans(struct motorRecord *); -STATIC long VELO = 0; struct motor_dset devPM304 = { @@ -166,10 +165,17 @@ STATIC RTN_STATUS PM304_end_trans(struct motorRecord *mr) } /* request homing move */ -STATIC void request_home(struct mess_node *motor_call, int model, int axis, int home_direction, int home_mode, char* datum_mode) { - // Max creep speed is 800 - set to velo if under 800 - int creep_speed = (VELO>800) ? 800 : VELO; - char buff[30]; +STATIC void request_home(struct mess_node *motor_call, int model, int axis, int home_direction, + struct PM304controller* cntrl) { + // hardware homes use creep speed. Max creep speed is 800, use velo if under this. Previous creep speed + // is restored next velocity change to cntrl->creep_speeds so previous value is used for any creep steps + // at end of regular motion + const int MAX_CREEP_SPEED = 800; + int velo = cntrl->velo[axis-1]; + int creep_speed = (velo>MAX_CREEP_SPEED) ? MAX_CREEP_SPEED : velo; + int home_mode = cntrl->home_mode[axis-1]; + char* datum_mode = cntrl->datum_mode[axis-1]; + char buff[32]; buff[0] = '\0'; if (model == MODEL_PM304){ sprintf(buff, "%dSC%d;", axis, creep_speed); @@ -209,6 +215,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo struct mess_node *motor_call; struct controller *brdptr; struct PM304controller *cntrl; + const double MAX_SERVO_PID = 32767.0; char buff[30]; int axis, card; RTN_STATUS rtnval; @@ -276,13 +283,13 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dMR%ld;", axis, ival); break; case HOME_REV: - request_home(motor_call, cntrl->model, axis, -1, cntrl->home_mode[axis-1], cntrl->datum_mode[axis-1]); + request_home(motor_call, cntrl->model, axis, -1, cntrl); break; case HOME_FOR: - request_home(motor_call, cntrl->model, axis, 1, cntrl->home_mode[axis-1], cntrl->datum_mode[axis-1]); + request_home(motor_call, cntrl->model, axis, 1, cntrl); break; case LOAD_POS: - sprintf(buff, "%dCP%ld;", axis, ival); /* check if need to scale by encoder ratio, think not ? */ + sprintf(buff, "%dCP%ld;", axis, ival); if (cntrl->use_encoder[axis-1] || cntrl->model != MODEL_PM304){ strcat(motor_call->message, buff); sprintf(buff, "%dAP%ld;", axis, ival); @@ -296,7 +303,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo strcat(motor_call->message, buff); } sprintf(buff, "%dSV%ld;", axis, ival); - VELO = ival; + cntrl->velo[axis-1] = ival; break; case SET_ACCEL: sprintf(buff, "%dSA%ld;", axis, ival); @@ -345,14 +352,14 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo } break; case SET_PGAIN: - // for servo mode we need NINT(dval * 32767.0) + // for servo mode we need NINT(dval * MAX_SERVO_PID) // in stepper mode this is used for end of move position correction if (cntrl->model == MODEL_PM304) { sprintf(buff, "%dKP%ld;", axis, ival); } else if (cntrl->control_mode[axis-1] == 1) { - sprintf(buff, "%dKP%ld;", axis, NINT(dval * 32767.0)); /* servo */ + sprintf(buff, "%dKP%ld;", axis, NINT(dval * MAX_SERVO_PID)); // servo motor, proportional gain servo coefficient } else { - sprintf(buff, "%dKP%ld;", axis, NINT(dval * 100.0)); /* stepper */ + sprintf(buff, "%dKP%ld;", axis, NINT(dval * 100.0)); // stepper motor, a correction gain percentage } break; @@ -360,7 +367,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo if (cntrl->model == MODEL_PM304) { sprintf(buff, "%dKS%ld;", axis, ival); } else if (cntrl->control_mode[axis-1] == 1) { - sprintf(buff, "%dKS%ld;", axis, NINT(dval * 32767.0)); // only valid in servo mode on PM600 + sprintf(buff, "%dKS%ld;", axis, NINT(dval * MAX_SERVO_PID)); // only valid in servo mode on PM600 } break; @@ -368,7 +375,7 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo if (cntrl->model == MODEL_PM304) { sprintf(buff, "%dKV%ld;", axis, ival); } else if (cntrl->control_mode[axis-1] == 1) { - sprintf(buff, "%dKV%ld;", axis, NINT(dval * 32767.0)); // only valid in servo mode on PM600 + sprintf(buff, "%dKV%ld;", axis, NINT(dval * MAX_SERVO_PID)); // only valid in servo mode on PM600 } break; @@ -380,27 +387,24 @@ STATIC RTN_STATUS PM304_build_trans(motor_cmnd command, double *parms, struct mo sprintf(buff, "%dAB;", axis); break; - /* limits may or may not be enforced depending on last SL command - hardware will not let you set a low limit >= high limit so order of setting - may be important. Need to look more closely at motor record */ + /* limits may or may not be enforced depending on last SL command. + Hardware will not let you set a low limit >= high limit so order of setting + may be important, but might get round that by just sending twice i.e set_low, set_high, set_low, set_high + Need to look more closely at motor record, it may already cover this if it gets a correct readback of existing limits + which it does not currently get. + sprintf(buff, "%dUL%ld;", axis, ival); // high limit + sprintf(buff, "%dLL%ld;", axis, ival); // low limit + */ case SET_HIGH_LIMIT: - if (false) { - sprintf(buff, "%dUL%ld;", axis, ival); - } else { - trans->state = IDLE_STATE; /* No command sent to the controller. */ - /* The PM304 internal soft limits are very difficult to retrieve, not - * implemented yet */ - } + trans->state = IDLE_STATE; /* No command sent to the controller. */ + /* The PM304 internal soft limits are very difficult to retrieve, not + * implemented yet */ break; case SET_LOW_LIMIT: - if (false) { - sprintf(buff, "%dLL%ld;", axis, ival); - } else { - trans->state = IDLE_STATE; /* No command sent to the controller. */ - /* The PM304 internal soft limits are very difficult to retrieve, not - * implemented yet */ - } + trans->state = IDLE_STATE; /* No command sent to the controller. */ + /* The PM304 internal soft limits are very difficult to retrieve, not + * implemented yet */ break; default: diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 126ebda26..eb26202ab 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -94,7 +94,7 @@ int controller_error = 0; /*----------------functions-----------------*/ STATIC int recv_mess(int card, char *buff, int len); STATIC RTN_STATUS send_mess(int, const char *, char *); -STATIC int send_recv_mess(int card, const char *out, char *in); +STATIC int send_recv_mess(int card, const char *out, char *in, size_t in_size); STATIC void start_status(int card); STATIC int set_status(int card, int signal); static long report(int level); @@ -174,7 +174,7 @@ static long report(int level) int card, motor_index; struct PM304controller *cntrl; char command[BUFF_SIZE]; - char buff[1024]; /* need bigger than usual as QA can return a lot */ + char buff[2048]; /* need bigger than usual as QA can return a lot */ if (PM304_num_cards <=0) printf(" NO PM304/PM600 controllers found\n"); @@ -199,18 +199,19 @@ static long report(int level) printDatumMode(cntrl->datum_mode[motor_index]); printAbortMode(cntrl->abort_mode[motor_index]); sprintf(command, "%dQM", motor_index+1); - send_recv_mess(card, command, buff); + send_recv_mess(card, command, buff, sizeof(buff)); printf("%s\n", buff); sprintf(command, "%dQS", motor_index+1); - send_recv_mess(card, command, buff); + send_recv_mess(card, command, buff, sizeof(buff)); printf("%s\n", buff); sprintf(command, "%dQP", motor_index+1); - send_recv_mess(card, command, buff); + send_recv_mess(card, command, buff, sizeof(buff)); printf("%s\n", buff); - //disable QA for moment - //sprintf(command, "%dQA", motor_index+1); - //send_recv_mess(card, command, buff); - //printf("%s\n", buff); + if (level > 0) { + sprintf(command, "%dQA", motor_index+1); + send_recv_mess(card, command, buff, sizeof(buff)); + printf("%s\n", buff); + } } } } @@ -282,7 +283,7 @@ STATIC int set_status(int card, int signal) /* Request the status of this motor */ sprintf(command, "%dOS;", signal+1); - send_recv_mess(card, command, response); + send_recv_mess(card, command, response, sizeof(response)); Debug(2, "set_status, status query, card %d, response=%s\n", card, response); status.Bits.RA_PLUS_LS = 0; @@ -346,7 +347,7 @@ STATIC int set_status(int card, int signal) if (cntrl->model != MODEL_PM304) { char *op; sprintf(command, "%dCO", signal+1); - send_recv_mess(card, command, response); + send_recv_mess(card, command, response, sizeof(response)); Debug(2, "set_status, operation query, card %d, response=%s\n", card, response); /* returns 01:XXX */ op = strchr(response, ':'); @@ -370,7 +371,7 @@ STATIC int set_status(int card, int signal) } else { sprintf(command, "%dOC;", signal+1); } - send_recv_mess(card, command, response); + send_recv_mess(card, command, response, sizeof(response)); /* Parse the response string which is of the form "AP=10234" (PM304) or 01:10234 (PM600)*/ motorData = atoi(&response[3]); Debug(2, "set_status, position query, card %d, response=%s\n", card, response); @@ -579,7 +580,7 @@ STATIC int recv_mess(int card, char *com, int flag) /* ring buffer */ /* send_recv_mess() */ /*****************************************************/ -STATIC int send_recv_mess(int card, const char *out, char *response) +STATIC int send_recv_mess(int card, const char *out, char *response, size_t response_maxsize) { char *p, *tok_save = NULL; struct PM304controller *cntrl; @@ -610,11 +611,14 @@ STATIC int send_recv_mess(int card, const char *out, char *response) response[0] = '\0'; Debug(2, "send_recv_mess: sending message to card %d, message=%s\n", card, p); status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), - response, BUFF_SIZE, TIMEOUT, + response, response_maxsize, TIMEOUT, &nwrite, &nread, &eomReason); /* The response from the PM304 is terminated with CR/LF. Remove these */ - if (nread == 0) response[0] = '\0'; + + if (nread < response_maxsize) { + response[nread] = '\0'; + } if (strchr(response, '!')) { level = 1; } @@ -630,8 +634,7 @@ STATIC int send_recv_mess(int card, const char *out, char *response) if (cntrl->model == MODEL_PM600) { pos = strchr(response, '\r'); if (pos != NULL) { - strcpy(temp, pos + 1); - strcpy(response, temp); + memmove(response, pos + 1, strlen(pos + 1) + 1); // +1 to copy string and NULL terminator } } } @@ -767,7 +770,7 @@ STATIC int motor_init() do { - send_recv_mess(card_index, "1OA;", buff); + send_recv_mess(card_index, "1OA;", buff, sizeof(buff)); retry++; /* Return value is length of response string */ } while(strlen(buff) == 0 && retry < 3); @@ -784,7 +787,7 @@ STATIC int motor_init() /* Don't turn on motor power, too dangerous */ /* send_mess(i, "1RSES;", buff); */ send_mess(card_index, "1ST;", 0); /* Stop motor */ - send_recv_mess(card_index, "1ID;", buff); /* Read controller ID string */ + send_recv_mess(card_index, "1ID;", buff, sizeof(buff)); /* Read controller ID string */ strncpy(brdptr->ident, buff, MAX_IDENT_LEN); /* Parse the response to figure out what model this is */ if (strstr(brdptr->ident, "PM304") != NULL) { @@ -807,7 +810,7 @@ STATIC int motor_init() motor_info->position = 0; if (cntrl->model != MODEL_PM304) { sprintf(command, "%dQM", motor_index+1); - send_recv_mess(card_index, command, buff); /* 01:CM = 1 AM = 00000000 DM = 00010000 JM = 11000000 */ + send_recv_mess(card_index, command, buff, sizeof(buff)); /* 01:CM = 1 AM = 00000000 DM = 00010000 JM = 11000000 */ if (strchr(buff, '=') != NULL) { cntrl->control_mode[motor_index] = atoi(strchr(buff, '=') + 1); } @@ -822,7 +825,7 @@ STATIC int motor_init() } /* Figure out if we have an encoder or not. If so use 0A, else use OC for readback. */ sprintf(command, "%dID", motor_index+1); - send_recv_mess(card_index, command, buff); /* Read controller ID string for this axis */ + send_recv_mess(card_index, command, buff, sizeof(buff)); /* Read controller ID string for this axis */ if (cntrl->model == MODEL_PM304) { /* For now always assume encoder if PM304 - needs work */ cntrl->use_encoder[motor_index] = 1; @@ -835,7 +838,7 @@ STATIC int motor_init() } /* Querying speeds for this axis */ sprintf(command, "%dQS", motor_index+1); - send_recv_mess(card_index, command, buff); + send_recv_mess(card_index, command, buff, sizeof(buff)); /* P600 returns 01:SC = 700 SV = 16200 SA = 50000 SD = 100000 LD = 200000 PM304 returns SV=16200,SC=1000,SA=100000,SD=100000 */ const char* delim = (cntrl->model == MODEL_PM304 ? "=," : "=: "); diff --git a/motorApp/MclennanSrc/drvPM304.h b/motorApp/MclennanSrc/drvPM304.h index 0262423bb..f95af5374 100644 --- a/motorApp/MclennanSrc/drvPM304.h +++ b/motorApp/MclennanSrc/drvPM304.h @@ -44,6 +44,7 @@ struct PM304controller int control_mode[PM304_MAX_CHANNELS]; /* PM600: datum mode as per CM command */ char datum_mode[PM304_MAX_CHANNELS][9]; /* PM600: datum mode as per DM command */ char abort_mode[PM304_MAX_CHANNELS][9]; /* PM600: abort mode as per AM command */ + int velo[PM304_MAX_CHANNELS]; /* last set velocity, used in homing to set creep speed */ }; RTN_STATUS PM304Setup(int, int); From 89d30ad78b4b98a79f656300eca280eb1597f82c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 12 May 2023 14:40:53 +0100 Subject: [PATCH 208/232] update comment on RA_HOME etc --- motorApp/MclennanSrc/drvPM304.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index eb26202ab..218c8ca84 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -339,9 +339,10 @@ STATIC int set_status(int card, int signal) if (response[3] == '1') { status.Bits.RA_MINUS_LS = 1; /* need to set ls_active = true; ? */ } - // [5] seems to be on most of the time -// status.Bits.RA_HOME = (response[5] == '1') ? 1 : 0; -// status.Bits.EA_HOME = (response[5] == '1') ? 1 : 0; + + // response[5] seems to be 1 all the time. Feels like you should be able + // to set the ATHOME bits based on it, but seems not. Maybe it refers to + // which side iof the signal you are on etc. as opposed to the transition } if (cntrl->model != MODEL_PM304) { From a009c2f1cf1b3311fa7340ac676b61b4e8d4191b Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sat, 13 May 2023 17:41:31 +0100 Subject: [PATCH 209/232] NULL terminate if read maxsize --- motorApp/MclennanSrc/drvPM304.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 218c8ca84..ce09a0b6d 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -619,7 +619,9 @@ STATIC int send_recv_mess(int card, const char *out, char *response, size_t resp if (nread < response_maxsize) { response[nread] = '\0'; - } + } else { + response[response_maxsize-1] = '\0'; + } if (strchr(response, '!')) { level = 1; } From 66aa86297cc58f97fe10ed4f019a2bedb48a612d Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Sat, 13 May 2023 18:14:04 +0100 Subject: [PATCH 210/232] Make sure string is terminated --- motorApp/MclennanSrc/drvPM304.cc | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index ce09a0b6d..718a84bf5 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -452,7 +452,7 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) /* Device support can send us multiple commands separated with ';' * characters. The PM304 cannot handle more than 1 command on a line * so send them separately */ - strcpy(temp, com); + strncpy(temp, com, sizeof(temp)); for (p = epicsStrtok_r(temp, ";", &tok_save); ((p != NULL) && (strlen(p) != 0)); p = epicsStrtok_r(NULL, ";", &tok_save)) { @@ -465,8 +465,12 @@ STATIC RTN_STATUS send_mess(int card, const char *com, char *name) /* Set the debug level for most responses to be 2. Flag reset messages * to 1 so we can spot them more easily */ int level = 2; + if (nread < BUFF_SIZE) { + response[nread] = '\0'; + } else { + response[BUFF_SIZE-1] = '\0'; + } if (nread == 0) { - response[0] = '\0'; Debug(1, "send_mess: card %d message=%s read ERROR: no response\n", card, p); } if (strchr(response, '!')) { /* an error contains an ! */ @@ -542,7 +546,11 @@ STATIC int recv_mess(int card, char *com, int flag) timeout, &nread, &eomReason); /* The response from the PM304 is terminated with CR/LF. Remove these */ - if (nread == 0) com[0] = '\0'; + if (nread < BUFF_SIZE) { + com[nread] = '\0'; + } else { + com[BUFF_SIZE-1] = '\0'; + } if (nread > 0) { if (strchr(com, '!')) { /* errors contain ! */ level = 1; From c056f180d2c4a00fa4aa9d6a5ae73948fbc33e2d Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 19 May 2023 01:00:33 +0100 Subject: [PATCH 211/232] Fix QA output --- motorApp/MclennanSrc/drvPM304.cc | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/motorApp/MclennanSrc/drvPM304.cc b/motorApp/MclennanSrc/drvPM304.cc index 718a84bf5..e2e297597 100644 --- a/motorApp/MclennanSrc/drvPM304.cc +++ b/motorApp/MclennanSrc/drvPM304.cc @@ -208,10 +208,18 @@ static long report(int level) send_recv_mess(card, command, buff, sizeof(buff)); printf("%s\n", buff); if (level > 0) { + // output from QA is many line, so temporarily disable \r\n termination checking and rely on TIMEOUT sprintf(command, "%dQA", motor_index+1); - send_recv_mess(card, command, buff, sizeof(buff)); - printf("%s\n", buff); - } + printf("Gathering output from %s command - this may cause a short pause...\n", command); + char eosSave[4]; + int eoslen = 0; + if (pasynOctetSyncIO->getInputEos(cntrl->pasynUser, eosSave, sizeof(eosSave), &eoslen) == asynSuccess) { + pasynOctetSyncIO->setInputEos(cntrl->pasynUser, "", 0); + send_recv_mess(card, command, buff, sizeof(buff)); + pasynOctetSyncIO->setInputEos(cntrl->pasynUser, eosSave, eoslen); + printf("%s\n", buff); + } + } } } } From 2d1aa63a37c072dadd87ff01754aa38714df904b Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 22 May 2023 15:47:50 +0100 Subject: [PATCH 212/232] Add HOMF and HOMR to ASL0 --- motorApp/MotorSrc/motorRecord.dbd | 2 ++ 1 file changed, 2 insertions(+) diff --git a/motorApp/MotorSrc/motorRecord.dbd b/motorApp/MotorSrc/motorRecord.dbd index 0e2d11c6b..143c12744 100644 --- a/motorApp/MotorSrc/motorRecord.dbd +++ b/motorApp/MotorSrc/motorRecord.dbd @@ -459,12 +459,14 @@ recordtype(motor) { interest(1) } field(HOMF,DBF_SHORT) { + asl(ASL0) prompt("Home Forward") special(SPC_MOD) pp(TRUE) interest(1) } field(HOMR,DBF_SHORT) { + asl(ASL0) prompt("Home Reverse") special(SPC_MOD) pp(TRUE) From 83b72930cbe9926acf633cea5c3fd4b97b37e02f Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 29 Dec 2023 19:02:17 +0000 Subject: [PATCH 213/232] Tidy up quotes --- motorApp/Db/motorStatus.db | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motorApp/Db/motorStatus.db b/motorApp/Db/motorStatus.db index 1993e1552..d6675a72c 100644 --- a/motorApp/Db/motorStatus.db +++ b/motorApp/Db/motorStatus.db @@ -41,6 +41,6 @@ record(ai, "$(P)$(M):DMOV") } record(stringin, "$(P)$(M)_IOCNAME"){ - field(DESC, "IOC name eg GALIL_01 for mtr rec") - field(VAL, "$(IOCNAME="UNKNOWN")") + field(DESC, "IOC name eg GALIL_01") + field(VAL, "$(IOCNAME=UNKNOWN)") } From e2baea7eb7d0e0d67ac9e5dfa141e70e660a4be7 Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Thu, 21 Dec 2023 16:03:55 +0000 Subject: [PATCH 214/232] move IN_POSITION records to motorstatus.db rather than axis.db (#62) --- motorApp/Db/motorStatus.db | 49 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/motorApp/Db/motorStatus.db b/motorApp/Db/motorStatus.db index d6675a72c..d1bb4e456 100644 --- a/motorApp/Db/motorStatus.db +++ b/motorApp/Db/motorStatus.db @@ -44,3 +44,52 @@ record(stringin, "$(P)$(M)_IOCNAME"){ field(DESC, "IOC name eg GALIL_01") field(VAL, "$(IOCNAME=UNKNOWN)") } + + +# Fallback tolerance used if neither retry deadband not setpoint deadband is set. +record(calc, "$(P)$(M):IN_POSITION:_FALLBACK") { + field(ASG, "READONLY") + field(INPA, "$(P)$(M).ERES CP MS") + field(INPB, "$(P)$(M).MRES CP MS") + field(CALC, "10*MAX(ABS(A),ABS(B))") +} + +record(calc, "$(P)$(M):IN_POSITION:TOLERANCE") { + field(ASG, "READONLY") + field(INPA, "$(P)$(M).RTRY CP MS") # Max retries + field(INPB, "$(P)$(M).RDBD CP MS") # Retry deadbands + field(INPC, "$(P)$(M).SPDB CP MS") # Setpoint deadband + field(INPD, "$(P)$(M).MRES CP MS") # motor resolution + field(INPE, "$(P)$(M):IN_POSITION:_FALLBACK CP MS") # fallback tolerance + + # If retry deadband > MRES or max retries > 0, use retry deadband + # Otherwise, if setpoint deadband is non-zero use it, else fall back to 10*MAX(ERES, MRES) + field(CALC, "(A>0||B>D)?B:((C>0)?C:E)") +} + +record(calc, "$(P)$(M):DIFF_WITHIN_TOL") { + field(ASG, "READONLY") + field(INPA, "$(P)$(M):IN_POSITION:TOLERANCE CP MS") + field(INPB, "$(P)$(M).DIFF CP MS") + + # If moving assume in position, otherwise check that + field(CALC, "ABS(B)<=ABS(A)") +} + +record(calc, "$(P)$(M):IN_POSITION") { + field(ASG, "READONLY") + field(INPA, "$(P)$(M):DIFF_WITHIN_TOL CP MS") + field(INPB, "$(P)$(M).MOVN CP MS") + + # If moving assume in position, otherwise check that + field(CALC, "B||A") +} + +record(calc, "$(P)$(M):USING_ENCODER") { + field(ASG, "READONLY") + field(INPA, "$(P)$(M).MSTA CP MS") + field(INPB, "$(P)$(M).UEIP CP MS") + + # Bitwise AND on MSTA to select the "encoder present" flag. + field(CALC, "((A&256)=256)&&B") +} From 75cc0446ec58e29122afb3d3145dbcf6d0ef26fb Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 23 Aug 2023 14:42:00 +0100 Subject: [PATCH 215/232] Disable smartArc for VS2010 --- modules/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/Makefile b/modules/Makefile index 13b5cdc51..0d74b5570 100644 --- a/modules/Makefile +++ b/modules/Makefile @@ -38,7 +38,7 @@ SUBMODULES += motorPIGCS2 ifdef LUA SUBMODULES += motorScriptMotor endif -SUBMODULES += motorSmarAct +#SUBMODULES += motorSmarAct SUBMODULES += motorSmartMotor SUBMODULES += motorThorLabs From 3e45ad8dd1df22146a89c13e77455eba6da93fcb Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 23 Aug 2023 00:10:15 +0100 Subject: [PATCH 216/232] Makde sure position init is disabled --- motorApp/MotorSrc/devMotorAsyn.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/motorApp/MotorSrc/devMotorAsyn.c b/motorApp/MotorSrc/devMotorAsyn.c index 4079ca38e..7ae6b306e 100644 --- a/motorApp/MotorSrc/devMotorAsyn.c +++ b/motorApp/MotorSrc/devMotorAsyn.c @@ -207,7 +207,12 @@ static void init_controller(struct motorRecord *pmr, asynUser *pasynUser ) initPos = 1; break; } - initPos = 0; /* never set position */ + + /* + * STFC ISIS: force off, but we should now change our dbs and use RSTM = Never (0) + */ + initPos = 0; + if (initPos) { double setPos = pmr->dval / pmr->mres; From 98e2a19e276c66ec0151b87ec26aa3b760f46387 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Thu, 15 Jun 2023 21:58:03 +0100 Subject: [PATCH 217/232] Make _able UDFS of NO_ALARM --- motorApp/Db/asyn_motor.db | 1 + motorApp/Db/asyn_motor_model2.db | 1 + motorApp/Db/basic_asyn_motor.db | 1 + motorApp/Db/motor.db | 1 + motorApp/Db/pseudoMotor.db | 1 + 5 files changed, 5 insertions(+) diff --git a/motorApp/Db/asyn_motor.db b/motorApp/Db/asyn_motor.db index cea9b7ea8..9991b78a8 100644 --- a/motorApp/Db/asyn_motor.db +++ b/motorApp/Db/asyn_motor.db @@ -40,6 +40,7 @@ record(bo, "$(P)$(M)_able") { field(OUT, "$(P)$(M).DISP") field(ZNAM, "Enable") field(ONAM, "Disable") + field(UDFS, "NO_ALARM") } record(calcout, "$(P)$(M)_vCh") { diff --git a/motorApp/Db/asyn_motor_model2.db b/motorApp/Db/asyn_motor_model2.db index 3fa391a54..f7e9254f1 100644 --- a/motorApp/Db/asyn_motor_model2.db +++ b/motorApp/Db/asyn_motor_model2.db @@ -33,6 +33,7 @@ record(bo, "$(P)$(M)_able") { field(OUT, "$(P)$(M).DISP") field(ZNAM, "Enable") field(ONAM, "Disable") + field(UDFS, "NO_ALARM") } record(calcout, "$(P)$(M)_vCh") { diff --git a/motorApp/Db/basic_asyn_motor.db b/motorApp/Db/basic_asyn_motor.db index e1cf368a4..b29fbd335 100644 --- a/motorApp/Db/basic_asyn_motor.db +++ b/motorApp/Db/basic_asyn_motor.db @@ -68,6 +68,7 @@ record(bo, "$(P)$(M)_able") { field(OUT, "$(P)$(M).DISP") field(ZNAM, "Enable") field(ONAM, "Disable") + field(UDFS, "NO_ALARM") } record(ao,"$(P)$(M)_MRESMON") diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 332aebdcc..6759975af 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -41,6 +41,7 @@ record(bo, "$(P)$(M)_able") { field(OUT, "$(P)$(M).DISP") field(ZNAM, "Enable") field(ONAM, "Disable") + field(UDFS, "NO_ALARM") } record(calcout, "$(P)$(M)_vCh") { diff --git a/motorApp/Db/pseudoMotor.db b/motorApp/Db/pseudoMotor.db index bcf431b5d..3bced055f 100644 --- a/motorApp/Db/pseudoMotor.db +++ b/motorApp/Db/pseudoMotor.db @@ -5,6 +5,7 @@ grecord(bo,"$(P)$(M)_able") field(OUT,"$(P)$(M)_ableput.A PP MS") field(ZNAM,"Enable") field(ONAM,"Disable") + field(UDFS, "NO_ALARM") } grecord(motor,"$(P)$(M)") { From 773a3f0582f852a92e84e4e5d833c29347f57e36 Mon Sep 17 00:00:00 2001 From: vhn15725 Date: Mon, 26 Jun 2023 14:22:36 +0100 Subject: [PATCH 218/232] Added recorded for scanning - SAFE_STUP --- motorApp/Db/periodic_polling.db | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/motorApp/Db/periodic_polling.db b/motorApp/Db/periodic_polling.db index c416160b3..28c1597ff 100644 --- a/motorApp/Db/periodic_polling.db +++ b/motorApp/Db/periodic_polling.db @@ -1,4 +1,4 @@ -record(calcout, "$(P)$(M):SCAN") { +record(calcout, "$(P)$(M):SAFE_STUP") { field(DTYP, "Soft Channel") field(DESC, "Polls the motor position") field(INPA, "$(P)$(M).MOVN") @@ -8,7 +8,14 @@ record(calcout, "$(P)$(M):SCAN") { field(INPE, "$(P)$(M).JOGF") field(INPF, "$(P)$(M).JOGR") field(CALC, "(A=0)&&(B=1)&&(C=0)&&(D=0)&&(E=0)&&(F=0)") - field(SCAN, "$(POLL_RATE=10) second") field(OOPT, "When Non-zero") field(OUT, "$(P)$(M).STUP PP") } + +record(ao, "$(P)$(M):SCAN") { + field(DTYP, "Soft Channel") + field(DESC, "Every POLL_RATE calls SAFE_STUP") + field(SCAN, "$(POLL_RATE=10) second") + field(FLNK, "$(P)$(M):SAFE_STUP.PROC") +} + From a6222c7126492945aa59582e3502a7f3e1178020 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 30 Jun 2023 17:08:18 +0100 Subject: [PATCH 219/232] Add indirection in case .PROC field not written to --- motorApp/Db/periodic_polling.db | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/motorApp/Db/periodic_polling.db b/motorApp/Db/periodic_polling.db index 28c1597ff..87f538eba 100644 --- a/motorApp/Db/periodic_polling.db +++ b/motorApp/Db/periodic_polling.db @@ -1,6 +1,10 @@ -record(calcout, "$(P)$(M):SAFE_STUP") { +record(bo, "$(P)$(M):SAFE_STUP") { + field(FLNK, "$(P)$(M):_SAFE_STUP.PROC") +} + +record(calcout, "$(P)$(M):_SAFE_STUP") { field(DTYP, "Soft Channel") - field(DESC, "Polls the motor position") + field(DESC, "Force Poll of motor position") field(INPA, "$(P)$(M).MOVN") field(INPB, "$(P)$(M).DMOV") field(INPC, "$(P)$(M).HOMF") From ef60458afa204c397adc2a0b3343dff05b27c624 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 7 Jul 2025 16:09:06 +0100 Subject: [PATCH 220/232] Remove moved files --- motorApp/MclennanSrc/homing.dbd | 1 - motorApp/MclennanSrc/homing.st | 227 -------------------------- motorApp/MclennanSrc/seqPVmacros.h | 245 ----------------------------- 3 files changed, 473 deletions(-) delete mode 100644 motorApp/MclennanSrc/homing.dbd delete mode 100644 motorApp/MclennanSrc/homing.st delete mode 100644 motorApp/MclennanSrc/seqPVmacros.h diff --git a/motorApp/MclennanSrc/homing.dbd b/motorApp/MclennanSrc/homing.dbd deleted file mode 100644 index 160f10e8c..000000000 --- a/motorApp/MclennanSrc/homing.dbd +++ /dev/null @@ -1 +0,0 @@ -registrar(homingRegistrar) diff --git a/motorApp/MclennanSrc/homing.st b/motorApp/MclennanSrc/homing.st deleted file mode 100644 index 8a384431b..000000000 --- a/motorApp/MclennanSrc/homing.st +++ /dev/null @@ -1,227 +0,0 @@ -program homing("MOTPV=xxx,MODE,AXIS,DEBUG") - -#include "seqPVmacros.h" - -%% #include "string.h" -%% #include "errlog.h" -%% #include "float.h" -%% #include "stdlib.h" -%% #include "math.h" - -char* SNLtaskName; -int jog_forward_value, jog_reverse_value; -assign jog_forward_value to "{MOTPV}.JOGF"; -assign jog_reverse_value to "{MOTPV}.JOGR"; - -int set, foff; -double position_d; -assign set to "{MOTPV}.SET"; -assign position_d to "{MOTPV}.DVAL"; -assign foff to "{MOTPV}.FOFF"; - -int foff_value; -int debug_flag; -int debug; -int mode; -int axis; - -double cached_soft_high_limit; -double cached_soft_low_limit; - -/* Turn on run-time debug messages */ -option +d; - -/* Make code reentrant. This is needed to run more than one instance of this program. */ -option +r; - -/* PV definitions */ -PV(int, home_forward_pv, "{MOTPV}.HOMF", Monitor); -PV(int, home_reverse_pv, "{MOTPV}.HOMR", Monitor); -PV(int, movable, "{MOTPV}.DMOV", Monitor); -PV(int, user_stop, "{MOTPV}.STOP", Monitor); - -/* Need to use SNL queue synchronization to get all events here. */ -syncq user_stop 100; - -// Soft limits -PV(double, hlm, "{MOTPV}.HLM", Monitor); -PV(double, llm, "{MOTPV}.LLM", Monitor); - - -#define HOME_MODE_BUILTIN 0 -#define HOME_MODE_CONST_VELOCITY_MOVE 1 -#define HOME_MODE_REVERSE_HOME_AND_ZERO 2 -#define HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO 3 -#define HOME_MODE_FORWARD_HOME_AND_ZERO 4 - -ss motor -{ - state init - { - when () - { - /* See definitions of home modes above. */ - mode = atoi(macValueGet("MODE")); - axis = atoi(macValueGet("AXIS")); - debug = atoi(macValueGet("DEBUG")); - errlogSevPrintf(errlogInfo, "Sequencer: Homing mode for axis %i set to %i\n", axis, mode); - - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: Debug mode ON\n"); - } - } state ready - } - - state ready - { - when (home_reverse_pv==1 && (mode==HOME_MODE_CONST_VELOCITY_MOVE||mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) - { - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO reverse_home_requested\n", axis); - } - } state reverse_home_requested - - /* Mode 3 always reverse homes */ - when ((home_forward_pv==1) && (mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) - { - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO reverse_home_requested\n", axis); - } - } state reverse_home_requested - - when ((home_forward_pv==1) && (mode==HOME_MODE_CONST_VELOCITY_MOVE)) - { - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM ready TO forward_home_requested\n", axis); - } - } state forward_home_requested - - exit { - /* Cache soft limits and then remove them */ - cached_soft_high_limit = hlm; - cached_soft_low_limit = llm; - - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: Caching limits (high=%f, low=%f)\n", axis, cached_soft_high_limit, cached_soft_low_limit); - } - PVPUT(hlm, DBL_MAX); - PVPUT(llm, -DBL_MAX); - } - } - - state forward_home_requested - { - /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when (home_forward_pv==0 && (mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { - - jog_forward_value = 1; - pvPut(jog_forward_value); - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM forward_home_requested TO processing_move_request (constant velocity move)\n", axis); - } - } state processing_move_request - - /* No delay needed for modes other than 1 */ - when (!(mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM forward_home_requested TO processing_move_request\n", axis); - } - } state processing_move_request - } - - state reverse_home_requested - { - /* In modes 1 and 3 we need to wait for the home to be cancelled before requesting a jog */ - when (home_reverse_pv==0 && (mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO && home_forward_pv==0)) { - - jog_reverse_value = 1; - pvPut(jog_reverse_value); - - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM reverse_home_requested TO processing_move_request (constant velocity move)\n", axis); - } - } state processing_move_request - - /* No delay needed for modes other than 1 and 3 */ - when (!(mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO)) { - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM reverse_home_requested TO processing_move_request\n", axis); - } - } state processing_move_request - } - - state processing_move_request - { - when (movable==0) { - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM processing_move_request TO moving\n", axis); - } - - pvFlushQ(user_stop); - - } state moving - - /* If move doesn't start within 10 seconds, cancel home (we may be in a state where we can't move) */ - when(delay(10)){ - errlogSevPrintf(errlogMajor, "Sequencer: axis %i: Unable to start homing move. Cancelling home.\n", axis); - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM processing_move_request TO done\n", axis); - } state done - } - - state moving - { - /* Slightly funny syntax - check if variable was updated, if it was, - update user_stop with it's latest value and then check stop condition. */ - when (pvGetQ(user_stop) && user_stop == 1) - { - errlogSevPrintf(errlogMajor, "Sequencer: axis %i: User stop detected during home - will not define position.\n", axis); - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM moving TO done\n", axis); - } - } state done - - when (movable==1) - { - if (mode==HOME_MODE_REVERSE_HOME_AND_ZERO || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO || mode==HOME_MODE_FORWARD_HOME_AND_ZERO) { - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: Setting position to zero (modes 2 and 3)\n", axis); - } - - pvGet(foff); - foff_value = foff; - PVPUT(foff, 1); /* frozen offset for applying home position */ - PVPUT(set, 1); - - PVPUT(position_d, 0.0); - - PVPUT(set, 0); - PVPUT(foff, foff_value); /* restore previous offset mode */ - } - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM moving TO done\n", axis); - } - } state done - } - - state done { - when () { - if (mode==HOME_MODE_CONST_VELOCITY_MOVE || mode==HOME_MODE_CONST_VELOCITY_MOVE_AND_ZERO) { - jog_reverse_value = 0; - pvPut(jog_reverse_value); - jog_forward_value = 0; - pvPut(jog_forward_value); - } - - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: Reapplying cached limits (high=%f, low=%f)\n", axis, cached_soft_high_limit, cached_soft_low_limit); - } - /* Re-apply cached soft limits */ - PVPUT(hlm, cached_soft_high_limit); - PVPUT(llm, cached_soft_low_limit); - - if (debug) { - errlogSevPrintf(errlogInfo, "Sequencer: axis %i: FROM done TO ready\n", axis); - } - } state ready - } -} diff --git a/motorApp/MclennanSrc/seqPVmacros.h b/motorApp/MclennanSrc/seqPVmacros.h deleted file mode 100644 index 33b8a1aa9..000000000 --- a/motorApp/MclennanSrc/seqPVmacros.h +++ /dev/null @@ -1,245 +0,0 @@ -/*================================================================ - * - * seqPVmacros.h -- PV-related macros for EPICS State Notation - * Language (SNL) code development - * - * 2004-May-19 [jemian] merged all DEBUG_PRINTn macros into one macro - * 2003-Oct-01 [jemian] changed errlogPrintf to printf (can't use in interrupts) - * 2003-Mar-20 [jemian] changed pvSet to PVPUT (and pvSetStr to PVPUTSTR) - * 2003-Mar-19 [jemian] changed printf to errlogPrintf - * 2002-Sep-24 [tischler] added pvSetStr and DEBUG_PRINT (a la Jemian) - * 2002-Jun-04 [lazmo] renamed to seqPVmacros.h - * 2002-Mar-08 [lazmo] added pvSet() - * 2002-Feb-21 [lazmo] fixed comments - * 2001-Nov-24 [lazmo] added PVA & PVAA - * 2001-Oct-18 [lazmo] original - * - * Author: - * M. Laznovsky -- lazmo_at_slac.stanford.edu - * - *================================================================ - */ - -/*---------------------------------------------------------------- - * PV() -- declare a variable and the EPICS PV it's assigned to, with - * optional monitor & event flag. One line takes the place of - * the usual: - * - * int blah; - * assign blah ... - * monitor blah; - * evflag blahFLag; - * sync blah ... - * - * Format: - * - * PV( varType, varName, pvName, other ) - * - * Where: - * - * varType is int, short, float, etc. - * varName is the variable name, e.g. foo - * pvName is the associated PV, e.g. "XYZ:ETC:FOO" - * other is one of the following: - * NoMon - * Monitor - * EvFlag - * - * Examples: - * - * PV (int, gui_goo, "{STN}:GUI:GOO", NoMon); <-- no monitor - * PV (float, gui_fudge, "{STN}:GUI:FUDGE", Monitor); <-- w/monitor - * PV (short, gui_run, "{STN}:GUI:RUN", EvFlag); <-- w/monitor & event flag - * - *---------------------------------------------------------------- - */ - -#define PV(_TYPE_,_VAR_,_PV_,_OTHER_) \ - _TYPE_ _VAR_; \ - assign _VAR_ to _PV_ \ - _OTHER_ (_VAR_) - -/*---------------------------------------------------------------- - * PVA(), for single waveform rec or array of PVs - * - * "varName" becomes array of ; 3rd arg is number of elements - * - * examples: - * - * single waveform record: - * - * PVA (short, plot_x0, 32, "{STN}:DATA:PLOT:X0", NoMon); - * - * array of PVs: - * - * #define PVA_zap { \ - * "{STN}:GUI:ZAP1", \ - * "{STN}:GUI:ZAP2", \ - * "{STN}:GUI:ZAP3" \ - * } - * PVA (int, zap, 3, PVA_zap, EvFlag); - * - *---------------------------------------------------------------- - */ - -#define PVA(_TYPE_,_VAR_,_NELEM_,_PV_,_OTHER_) \ - _TYPE_ _VAR_ [ _NELEM_ ]; \ - assign _VAR_ to _PV_ \ - _OTHER_ (_VAR_) - -/*---------------------------------------------------------------- - * PVAA(), for arrays of waveform records - * - * "varName" becomes double-dimensioned array of - * 3rd arg is number of waveform records - * 4th arg is number of elements per record - * Need to define another macro first for "_PV_" - * - * example: - * - * #define PVAA_plotx { \ - * "{STN}:DATA:PLOT:X1", \ - * "{STN}:DATA:PLOT:X2", \ - * "{STN}:DATA:PLOT:X3" \ - * } - * PVAA (short, plotx, 3, 500, PVAA_plotx, NoMon); - * - *---------------------------------------------------------------- - */ - -#define PVAA(_TYPE_,_VAR_,_NREC_,_NELEM_,_PV_,_OTHER_) \ - _TYPE_ _VAR_ [ _NREC_ ] [ _NELEM_ ]; \ - assign _VAR_ to _PV_ \ - _OTHER_ (_VAR_) - -/*---------------------------------------------------------------- - * macros for last arg of PV* ("_OTHER_") - *---------------------------------------------------------------- - */ - -/* - * no monitor - */ -#define NoMon(_VAR_) /* this macro intentionally left blank :P */ - -/* - * monitor - */ -#define Monitor(_VAR_) ; monitor _VAR_ - -/* - * monitor & event flag; flag var will be named "_EvFlag" - */ -#define EvFlag(_VAR_) \ - Monitor (_VAR_); \ - evflag _VAR_##_mon; \ - sync _VAR_ _VAR_##_mon - -/*================================================================*/ -/*================================================================*/ - -/*---------------------------------------------------------------- - * PVPUT() -- variable assign and pvPut() in one - * - * Format: - * - * PVPUT( varName, expr ) - * - * Where: - * - * varName is the variable name, e.g. foo - * expr is any C expression - * - * Examples: - * - * PVPUT (foo, 3); - * expands to: - * foo = 3; - * pvPut(foo); - * - * PVPUT (bar, xyz + 2); - * expands to: - * bar = xyz + 2; - * pvPut(bar); - * - *---------------------------------------------------------------- - */ - -#define PVPUT(_VAR_,_EXPR_) \ - { \ - _VAR_ = ( _EXPR_ ); \ - pvPut(_VAR_); \ - } - -/*---------------------------------------------------------------- - * PVPUTSTR() -- string assign and pvPut() in one - * - * Format: - * - * PVPUTSTR( strName, string expr ) - * - * Where: - * - * strName is the variable name, e.g. foo - * string expr is any C expression that points to a string - * - * Example: - * - * PVPUTSTR (foo, "error message"); - * expands to: - * strcpy(foo,"error message"); - * pvPut(foo); - * - *---------------------------------------------------------------- - */ - -#define PVPUTSTR(_STR_,_EXPR_) \ - { \ - strcpy(_STR_,_EXPR_); \ - pvPut(_STR_); \ - } - -/*================================================================*/ -/*================================================================*/ - -/*---------------------------------------------------------------- - * DEBUG_PRINT() -- print a debug string for a particular debug level - * - * Format: - * - * DEBUG_PRINT( debug_level, string format, v1, v2, v3, v4 ) - * - * Where: - * - * debug_level is the debugging level, ie 3 - * string format is an sprintf format specifier - * v1, v2, v3, v4 are optional arguments for the sprintf format - * - * Example: - * - * DEBUG_PRINT (1, "ERROR, how did I get here?", 0, 0, 0, 0 ); - * - *---------------------------------------------------------------- - */ - -#if defined(NO_DEBUGGING_OUTPUT) -#define DEBUG_PRINT(DEBUG_LEVEL, FMT,V1,V2,V3,V4) ; -#else - -#define DEBUG_PRINT(DEBUG_LEVEL,FMT,V1,V2,V3,V4) \ - if (debug_flag >= DEBUG_LEVEL) { \ - printf("<%s,%d,%s,%d> ", \ - __FILE__, __LINE__, \ - SNLtaskName, DEBUG_LEVEL); \ - printf(FMT,V1,V2,V3,V4); \ - printf("\n"); \ - } - - -#endif - -/*================================================================*/ -/*================================================================*/ -/*================================================================*/ - -/* end */ From ab9a5220abffd99b8d132dc7df27c60e4faa449f Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 7 Jul 2025 23:15:07 +0100 Subject: [PATCH 221/232] Repoint submodules --- .gitmodules | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.gitmodules b/.gitmodules index d4ff47101..f7c46ae2a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "modules/motorNewport"] path = modules/motorNewport - url = https://github.com/epics-motor/motorNewport.git + url = https://github.com/ISISComputingGroup/motorNewport.git [submodule "modules/motorOms"] path = modules/motorOms url = https://github.com/epics-motor/motorOms.git @@ -24,7 +24,7 @@ url = https://github.com/epics-motor/motorKohzu.git [submodule "modules/motorMclennan"] path = modules/motorMclennan - url = https://github.com/epics-motor/motorMclennan.git + url = https://github.com/ISISComputingGroup/motorMclennan.git [submodule "modules/motorMicroMo"] path = modules/motorMicroMo url = https://github.com/epics-motor/motorMicroMo.git @@ -78,13 +78,13 @@ url = https://github.com/epics-motor/motorAMCI.git [submodule "modules/motorPIGCS2"] path = modules/motorPIGCS2 - url = https://github.com/epics-motor/motorPIGCS2.git + url = https://github.com/ISISComputingGroup/motorPIGCS2.git [submodule "modules/motorSmarAct"] path = modules/motorSmarAct url = https://github.com/epics-motor/motorSmarAct.git [submodule "modules/motorMotorSim"] path = modules/motorMotorSim - url = https://github.com/epics-motor/motorMotorSim.git + url = https://github.com/ISISComputingGroup/motorMotorSim.git [submodule "modules/motorMXmotor"] path = modules/motorMXmotor url = https://github.com/epics-motor/motorMXmotor.git From 28e972fe9e30d9ea6c32a21a48a4eb66f2d8ce78 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 7 Jul 2025 23:30:26 +0100 Subject: [PATCH 222/232] Update submodules --- modules/motorAcsMotion | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/motorAcsMotion b/modules/motorAcsMotion index ee68ab79d..91e8e1eb8 160000 --- a/modules/motorAcsMotion +++ b/modules/motorAcsMotion @@ -1 +1 @@ -Subproject commit ee68ab79daf075215312ad851a84f5f18697ce77 +Subproject commit 91e8e1eb8d7e468f5628482a9e18e891c5960d9f From c0a8c18fdd9d88a36520a03a6ccdbbc9130602a8 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 7 Jul 2025 23:39:42 +0100 Subject: [PATCH 223/232] Update submodules --- .ci | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.ci b/.ci index 20f8e0539..576460163 160000 --- a/.ci +++ b/.ci @@ -1 +1 @@ -Subproject commit 20f8e053931fdef8a9413cc6229286c5d9ed152f +Subproject commit 576460163031255c09948566dd2f2409d86ab8d4 From bd6eb6c54dc0ffef6e2614529376bfed13855b19 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Mon, 7 Jul 2025 23:41:32 +0100 Subject: [PATCH 224/232] Build fixes --- configure/RELEASE | 5 ----- modules/Makefile | 6 +++--- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/configure/RELEASE b/configure/RELEASE index 668058b93..f79c2a97f 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -62,8 +62,3 @@ LUA=$(SUPPORT)/lua/master include $(TOP)/../../../ISIS_CONFIG -include $(TOP)/../../../ISIS_CONFIG.$(EPICS_HOST_ARCH) - - - - ->>>>>>> R7-3 diff --git a/modules/Makefile b/modules/Makefile index 0d74b5570..4f23aebcb 100644 --- a/modules/Makefile +++ b/modules/Makefile @@ -7,8 +7,8 @@ SUBMODULES += motorOms SUBMODULES += motorNewport SUBMODULES += motorAMCI SUBMODULES += motorAcs -SUBMODULES += motorAcsMotion -SUBMODULES += motorAcsTech80 +#SUBMODULES += motorAcsMotion +#SUBMODULES += motorAcsTech80 SUBMODULES += motorAerotech SUBMODULES += motorAttocube SUBMODULES += motorDeltaTau @@ -19,7 +19,7 @@ endif SUBMODULES += motorIms SUBMODULES += motorKohzu SUBMODULES += motorMclennan -SUBMODULES += motorMicos +#SUBMODULES += motorMicos SUBMODULES += motorMicroMo SUBMODULES += motorMicronix SUBMODULES += motorMotorSim From e08f8d4c290ac9a2b95005537ce32cf01929b28c Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 8 Jul 2025 20:59:08 +0100 Subject: [PATCH 225/232] Build fixes --- .gitmodules | 17 +- modules/Makefile | 8 + modules/motorLinMot | 1 + modules/motorMclennan | 2 +- modules/motorMotorSim | 2 +- modules/motorNewport | 2 +- modules/motorSM300 | 1 + motorApp/LinMotSrc/LinMotRegister.cc | 52 --- motorApp/LinMotSrc/Makefile | 24 -- motorApp/LinMotSrc/devLinMot.cc | 235 ---------- motorApp/LinMotSrc/devLinMotMotor.dbd | 5 - motorApp/LinMotSrc/drvLinMot.cc | 481 --------------------- motorApp/LinMotSrc/drvLinMot.h | 44 -- motorApp/SM300Src/Makefile | 30 -- motorApp/SM300Src/SM300Driver.cpp | 599 -------------------------- motorApp/SM300Src/SM300Driver.h | 81 ---- motorApp/SM300Src/SM300Motor.dbd | 3 - motorApp/SM300Src/SM300Register.cc | 41 -- motorApp/SM300Src/SM300Register.h | 12 - 19 files changed, 26 insertions(+), 1614 deletions(-) create mode 160000 modules/motorLinMot create mode 160000 modules/motorSM300 delete mode 100644 motorApp/LinMotSrc/LinMotRegister.cc delete mode 100644 motorApp/LinMotSrc/Makefile delete mode 100644 motorApp/LinMotSrc/devLinMot.cc delete mode 100644 motorApp/LinMotSrc/devLinMotMotor.dbd delete mode 100644 motorApp/LinMotSrc/drvLinMot.cc delete mode 100644 motorApp/LinMotSrc/drvLinMot.h delete mode 100644 motorApp/SM300Src/Makefile delete mode 100644 motorApp/SM300Src/SM300Driver.cpp delete mode 100644 motorApp/SM300Src/SM300Driver.h delete mode 100644 motorApp/SM300Src/SM300Motor.dbd delete mode 100644 motorApp/SM300Src/SM300Register.cc delete mode 100644 motorApp/SM300Src/SM300Register.h diff --git a/.gitmodules b/.gitmodules index f7c46ae2a..2a0775ec3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ [submodule "modules/motorNewport"] path = modules/motorNewport url = https://github.com/ISISComputingGroup/motorNewport.git + branch = ibex [submodule "modules/motorOms"] path = modules/motorOms url = https://github.com/epics-motor/motorOms.git @@ -25,6 +26,7 @@ [submodule "modules/motorMclennan"] path = modules/motorMclennan url = https://github.com/ISISComputingGroup/motorMclennan.git + branch = ibex [submodule "modules/motorMicroMo"] path = modules/motorMicroMo url = https://github.com/epics-motor/motorMicroMo.git @@ -78,22 +80,29 @@ url = https://github.com/epics-motor/motorAMCI.git [submodule "modules/motorPIGCS2"] path = modules/motorPIGCS2 - url = https://github.com/ISISComputingGroup/motorPIGCS2.git + url = https://github.com/epics-motor/motorPIGCS2.git [submodule "modules/motorSmarAct"] path = modules/motorSmarAct url = https://github.com/epics-motor/motorSmarAct.git [submodule "modules/motorMotorSim"] path = modules/motorMotorSim url = https://github.com/ISISComputingGroup/motorMotorSim.git + branch = ibex [submodule "modules/motorMXmotor"] path = modules/motorMXmotor url = https://github.com/epics-motor/motorMXmotor.git [submodule "modules/motorParker"] path = modules/motorParker url = https://github.com/epics-motor/motorParker.git -[submodule ".ci"] - path = .ci - url = https://github.com/epics-base/ci-scripts [submodule "modules/motorAcsMotion"] path = modules/motorAcsMotion url = https://github.com/epics-motor/motorAcsMotion.git +[submodule ".ci"] + path = .ci + url = https://github.com/epics-base/ci-scripts +[submodule "modules/motorSM300"] + path = modules/motorSM300 + url = https://github.com/ISISComputingGroup/motorSM300.git +[submodule "modules/motorLinMot"] + path = modules/motorLinMot + url = https://github.com/ISISComputingGroup/motorLinMot.git diff --git a/modules/Makefile b/modules/Makefile index 4f23aebcb..30e86c09b 100644 --- a/modules/Makefile +++ b/modules/Makefile @@ -73,6 +73,14 @@ ifdef BUSY $(ECHO) Creating $@, BUSY = $(BUSY) @echo BUSY = $(BUSY)>> $@ endif +ifdef CALC + $(ECHO) Creating $@, CALC = $(CALC) + @echo CALC = $(CALC)>> $@ +endif +ifdef SSCAN + $(ECHO) Creating $@, SSCAN = $(SSCAN) + @echo SSCAN = $(SSCAN)>> $@ +endif ifdef IPAC $(ECHO) Creating $@, IPAC = $(IPAC) @echo IPAC = $(IPAC)>> $@ diff --git a/modules/motorLinMot b/modules/motorLinMot new file mode 160000 index 000000000..ca9417758 --- /dev/null +++ b/modules/motorLinMot @@ -0,0 +1 @@ +Subproject commit ca941775851a7e9265c5cfa58f1d86d58916d0f8 diff --git a/modules/motorMclennan b/modules/motorMclennan index dab26dc0d..2024aa22d 160000 --- a/modules/motorMclennan +++ b/modules/motorMclennan @@ -1 +1 @@ -Subproject commit dab26dc0d0bf709ca4b9d8c8aed73fdbfd75e500 +Subproject commit 2024aa22dc0a9b3ea8c53341ac74d94698aa8add diff --git a/modules/motorMotorSim b/modules/motorMotorSim index cdea87404..9382836c7 160000 --- a/modules/motorMotorSim +++ b/modules/motorMotorSim @@ -1 +1 @@ -Subproject commit cdea87404306161a0f9edf5cd6efd2410612b6f7 +Subproject commit 9382836c729c705de51ecfd6af57e2aa6e558995 diff --git a/modules/motorNewport b/modules/motorNewport index d84cfe2bf..395031053 160000 --- a/modules/motorNewport +++ b/modules/motorNewport @@ -1 +1 @@ -Subproject commit d84cfe2bf5247475cebd8ce609c219e854c72742 +Subproject commit 395031053f141d9980b3de489945ce7da47469cc diff --git a/modules/motorSM300 b/modules/motorSM300 new file mode 160000 index 000000000..14e61a563 --- /dev/null +++ b/modules/motorSM300 @@ -0,0 +1 @@ +Subproject commit 14e61a56308390127711df94fbf14fc4cefa749f diff --git a/motorApp/LinMotSrc/LinMotRegister.cc b/motorApp/LinMotSrc/LinMotRegister.cc deleted file mode 100644 index 79ac0bc66..000000000 --- a/motorApp/LinMotSrc/LinMotRegister.cc +++ /dev/null @@ -1,52 +0,0 @@ -/***************************************************************** - COPYRIGHT NOTIFICATION -***************************************************************** - -(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO - -This software was developed under a United States Government license -described on the COPYRIGHT_UniversityOfChicago file included as part -of this distribution. -**********************************************************************/ - -#include -#include "drvLinMot.h" -#include "epicsExport.h" - -extern "C" -{ - -// LinMotSetup arguments -static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; -static const iocshArg setupArg1 = {"Polling rate", iocshArgInt}; - -// LinMotConfig arguments -static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; -static const iocshArg configArg1 = {"asyn port name", iocshArgString}; -static const iocshArg configArg2 = {"Number of axes", iocshArgInt}; - -static const iocshArg * const LinMotSetupArgs[2] = {&setupArg0, &setupArg1}; -static const iocshArg * const LinMotConfigArgs[3] = {&configArg0, &configArg1, - &configArg2}; - -static const iocshFuncDef setupLinMot = {"LinMotSetup", 2, LinMotSetupArgs}; -static const iocshFuncDef configLinMot = {"LinMotConfig", 3, LinMotConfigArgs}; - -static void setupLinMotCallFunc(const iocshArgBuf *args) -{ - LinMotSetup(args[0].ival, args[1].ival); -} -static void configLinMotCallFunc(const iocshArgBuf *args) -{ - LinMotConfig(args[0].ival, args[1].sval, args[2].ival); -} - -static void LinMotRegister(void) -{ - iocshRegister(&setupLinMot, setupLinMotCallFunc); - iocshRegister(&configLinMot, configLinMotCallFunc); -} - -epicsExportRegistrar(LinMotRegister); - -} // extern "C" diff --git a/motorApp/LinMotSrc/Makefile b/motorApp/LinMotSrc/Makefile deleted file mode 100644 index eb1d5ab92..000000000 --- a/motorApp/LinMotSrc/Makefile +++ /dev/null @@ -1,24 +0,0 @@ -# Makefile -TOP = ../.. -include $(TOP)/configure/CONFIG - -# Both the following line, and a line in the *.dbd file, -# must be uncommented to use diagnostic debugging messages. -USR_CXXFLAGS += -DDEBUG - -DBD += devLinMotMotor.dbd - -LIBRARY_IOC = LinMot - -# Intelligent Motion Systems driver support. -SRCS += LinMotRegister.cc -SRCS += devLinMot.cc drvLinMot.cc - -LinMot_LIBS += motor -LinMot_LIBS += asyn -LinMot_LIBS += seq -LinMot_LIBS += pv -LinMot_LIBS += $(EPICS_BASE_IOC_LIBS) - -include $(TOP)/configure/RULES - diff --git a/motorApp/LinMotSrc/devLinMot.cc b/motorApp/LinMotSrc/devLinMot.cc deleted file mode 100644 index 550fc3cf1..000000000 --- a/motorApp/LinMotSrc/devLinMot.cc +++ /dev/null @@ -1,235 +0,0 @@ -#include -#include "motorRecord.h" -#include "motor.h" -#include "motordevCom.h" -#include "drvLinMot.h" -#include "epicsExport.h" - -#define STATIC static -#define ASCII_0_TO_A 65 /* ASCII offset between 0 and A */ -#define NINT(f) (long)((f)>0 ? (f)+0.5 : (f)-0.5) - -extern struct driver_table LinMot_access; - -/*----------------debugging-----------------*/ -volatile int devLinMotDebug = 0; -extern "C" {epicsExportAddress(int, devLinMotDebug);} - -static inline void Debug(int level, const char *format, ...) { - if (level < devLinMotDebug) { - va_list pVar; - va_start(pVar, format); - vprintf(format, pVar); - va_end(pVar); - } -} - -/* ----------------Create the dsets for devLinMot----------------- */ -STATIC struct driver_table *drvtabptr; -STATIC long LinMot_init(void *); -STATIC long LinMot_init_record(void *); -STATIC long LinMot_start_trans(struct motorRecord *); -STATIC RTN_STATUS LinMot_build_trans(motor_cmnd, double *, struct motorRecord *); -STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *); - -struct motor_dset devLinMot = -{ - {8, NULL, (DEVSUPFUN) LinMot_init, (DEVSUPFUN) LinMot_init_record, NULL}, - motor_update_values, - LinMot_start_trans, - LinMot_build_trans, - LinMot_end_trans -}; - -extern "C" {epicsExportAddress(dset,devLinMot);} - -/* --------------------------- program data --------------------- */ -/* This table is used to define the command types */ - -static msg_types LinMot_table[] = { - MOTION, /* MOVE_ABS */ - MOTION, /* MOVE_REL */ - MOTION, /* HOME_FOR */ - MOTION, /* HOME_REV */ - IMMEDIATE, /* LOAD_POS */ - IMMEDIATE, /* SET_VEL_BASE */ - IMMEDIATE, /* SET_VELOCITY */ - IMMEDIATE, /* SET_ACCEL */ - IMMEDIATE, /* GO */ - IMMEDIATE, /* SET_ENC_RATIO */ - INFO, /* GET_INFO */ - MOVE_TERM, /* STOP_AXIS */ - VELOCITY, /* JOG */ - IMMEDIATE, /* SET_PGAIN */ - IMMEDIATE, /* SET_IGAIN */ - IMMEDIATE, /* SET_DGAIN */ - IMMEDIATE, /* ENABLE_TORQUE */ - IMMEDIATE, /* DISABL_TORQUE */ - IMMEDIATE, /* PRIMITIVE */ - IMMEDIATE, /* SET_HIGH_LIMIT */ - IMMEDIATE /* SET_LOW_LIMIT */ -}; - -static struct board_stat **LinMot_cards; - -/* --------------------------- program data --------------------- */ - -/* initialize device support for LinMot stepper motor */ -STATIC long LinMot_init(void *arg) -{ - long rtnval; - int after = (arg == 0) ? 0 : 1; - - if (after == 0) - { - drvtabptr = &LinMot_access; - (drvtabptr->init)(); - } - - rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &LinMot_cards); - return(rtnval); -} - - -/* initialize a record instance */ -STATIC long LinMot_init_record(void *arg) -{ - struct motorRecord *mr = (struct motorRecord *) arg; - long rtnval; - - rtnval = motor_init_record_com(mr, *drvtabptr->cardcnt_ptr, - drvtabptr, LinMot_cards); - return(rtnval); -} - - -/* start building a transaction */ -STATIC long LinMot_start_trans(struct motorRecord *mr) -{ - long rtnval; - rtnval = motor_start_trans_com(mr, LinMot_cards); - return(rtnval); -} - - -/* end building a transaction */ -STATIC RTN_STATUS LinMot_end_trans(struct motorRecord *mr) -{ - RTN_STATUS rtnval; - rtnval = motor_end_trans_com(mr, drvtabptr); - return(rtnval); - -} - -/* add a part to the transaction */ -STATIC RTN_STATUS LinMot_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) -{ - struct motor_trans *trans = (struct motor_trans *) mr->dpvt; - struct mess_node *motor_call; - struct controller *brdptr; - struct LinMotController *cntrl; - char buff[30]; - int card; - char axis; - RTN_STATUS rtnval; - double dval; - long ival; - - rtnval = OK; - buff[0] = '\0'; - /* Protect against NULL pointer with WRTITE_MSG(GO/STOP_AXIS/GET_INFO, NULL). */ - dval = (parms == NULL) ? 0.0 : *parms; - ival = NINT(dval); - - motor_call = &(trans->motor_call); - card = motor_call->card; - axis = motor_call->signal + ASCII_0_TO_A; - brdptr = (*trans->tabptr->card_array)[card]; - if (brdptr == NULL) - return(rtnval = ERROR); - - cntrl = (struct LinMotController *) brdptr->DevicePrivate; - - if (LinMot_table[command] > motor_call->type) - motor_call->type = LinMot_table[command]; - - if (trans->state != BUILD_STATE) - return(rtnval = ERROR); - - if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0) - { - strcat(motor_call->message, mr->init); - strcat(motor_call->message, "\r"); - } - - switch (command) - { - case MOVE_ABS: - case MOVE_REL: - case HOME_FOR: - case HOME_REV: - case JOG: - if (strlen(mr->prem) != 0) - { - strcat(motor_call->message, mr->prem); - strcat(motor_call->message, ";"); - } - if (strlen(mr->post) != 0) - motor_call->postmsgptr = (char *) &mr->post; - break; - - default: - break; - } - - switch (command) - { - case MOVE_ABS: - sprintf(buff, "!SP%ld%c;", ival, axis); - break; - case SET_VELOCITY: - /* Speed resolution in nm/s. Convert to mm/s. */ - sprintf(buff, "!SV%ld%c;", int(ival*1000000.0/cntrl->speed_resolution), axis); - break; - case SET_ACCEL: - sprintf(buff, "!SA%ld%c;", ival, axis); - break; - case SET_PGAIN: - sprintf(buff, "!DP%ld%c;", ival, axis); - break; - case SET_IGAIN: - sprintf(buff, "!DI%ld%c;", ival, axis); - break; - case SET_DGAIN: - sprintf(buff, "!DD%ld%c;", ival, axis); - break; - case HOME_REV: - case HOME_FOR: - sprintf(buff, "!SR-1;"); - strcat(motor_call->message, buff); - sprintf(buff, "!SR+1;"); - break; - case LOAD_POS: - sprintf(buff, "!RP%ld%c;", ival, axis); - break; - /* These commands are unimplemented because there are no suitable motor commands - that fit them */ - case STOP_AXIS: - case GO: - case SET_ENC_RATIO: - case GET_INFO: - case JOG: - case MOVE_REL: - case SET_VEL_BASE: - case ENABLE_TORQUE: - case DISABL_TORQUE: - case SET_HIGH_LIMIT: - case SET_LOW_LIMIT: - break; - default: - rtnval = ERROR; - } - strcat(motor_call->message, buff); - Debug(3, "LinMot_build_trans: buff=%s, motor_call->message=%s\n", buff, motor_call->message); - return (rtnval); -} \ No newline at end of file diff --git a/motorApp/LinMotSrc/devLinMotMotor.dbd b/motorApp/LinMotSrc/devLinMotMotor.dbd deleted file mode 100644 index ad1d09f0e..000000000 --- a/motorApp/LinMotSrc/devLinMotMotor.dbd +++ /dev/null @@ -1,5 +0,0 @@ -# LinMot motor controller support. -device(motor,VME_IO, devLinMot, "LinMot") -driver(drvLinMot) -registrar(LinMotRegister) -variable(drvLinMotDebug) diff --git a/motorApp/LinMotSrc/drvLinMot.cc b/motorApp/LinMotSrc/drvLinMot.cc deleted file mode 100644 index a289e7d74..000000000 --- a/motorApp/LinMotSrc/drvLinMot.cc +++ /dev/null @@ -1,481 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include "motor.h" -#include "drvLinMot.h" -#include "asynOctetSyncIO.h" -#include "epicsExport.h" - -#define STATIC static -#define TIMEOUT 3.0 /* Command timeout in sec */ -#define ASCII_0_TO_A 65 /* ASCII offset between 0 and A */ -#define BUFF_SIZE 200 /* Maximum length of string to/from LinMot */ - -struct mess_queue -{ - struct mess_node *head; - struct mess_node *tail; -}; - -/*----------------debugging-----------------*/ -volatile int drvLinMotDebug = 0; -extern "C" {epicsExportAddress(int, drvLinMotDebug);} - -static inline void Debug(int level, const char *format, ...) { - if (level < drvLinMotDebug) { - va_list pVar; - va_start(pVar, format); - vprintf(format, pVar); - va_end(pVar); - } -} - -int LinMot_num_cards = 0; - -/* Local data required for every driver; see "motordrvComCode.h" */ -#include "motordrvComCode.h" - -/*----------------functions-----------------*/ -STATIC int recv_mess(int card, char *buff, int len); -STATIC RTN_STATUS send_mess(int, const char *, char *); -STATIC int send_recv_mess(int card, const char *out, char *in); -STATIC void start_status(int card); -STATIC int set_status(int card, int signal); -static long report(int level); -static long init(); -STATIC int motor_init(); -STATIC void query_done(int, int, struct mess_node *); - -/*----------------functions-----------------*/ -struct driver_table LinMot_access = -{ - motor_init, - motor_send, - motor_free, - motor_card_info, - motor_axis_info, - &mess_queue, - &queue_lock, - &free_list, - &freelist_lock, - &motor_sem, - &motor_state, - &total_cards, - &any_motor_in_motion, - send_mess, - recv_mess, - set_status, - query_done, - start_status, - &initialized, - NULL -}; - -struct drvLinMot_drvet -{ - long number; -#ifdef __cplusplus - long (*report) (int); - long (*init) (void); -#else - DRVSUPFUN report; - DRVSUPFUN init; -#endif -} drvLinMot = {2, report, init}; - -extern "C" {epicsExportAddress(drvet, drvLinMot);} - -STATIC struct thread_args targs = {SCAN_RATE, &LinMot_access, 0.0}; - -/********************************************************* - * Print out driver status report - *********************************************************/ -static long report(int level) -{ - if (LinMot_num_cards <=0) { - epicsPrintf("LinMot report: No ontrollers found\n"); - } - else { - for (int card = 0; card < LinMot_num_cards; card++) - if (motor_state[card]) - printf("LinMot controller %d active\n", - card); - } - return (OK); -} - -static long init() -{ - if (LinMot_num_cards <= 0) - { - Debug(1, "LinMotSetup() is missing from startup script.\n"); - return (ERROR); - } - return (OK); -} - -STATIC void query_done(int card, int axis, struct mess_node *nodeptr) -{ -} - -STATIC void start_status(int card) -{ -} - - -/************************************************************** - * Query position and status for an axis - **************************************************************/ -STATIC int set_status(int card, int signal) -{ - register struct mess_info *motor_info; - char command[BUFF_SIZE]; - char warning_response[BUFF_SIZE], error_response[BUFF_SIZE], position_response[BUFF_SIZE]; - struct mess_node *nodeptr; - int rtn_state; - long motorData; - char buff[BUFF_SIZE]; - struct LinMotController *cntrl; - msta_field status; - - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - - motor_info = &(motor_state[card]->motor_info[signal]); - nodeptr = motor_info->motor_motion; - status.All = motor_info->status.All; - - /* Request the status of this motor */ - sprintf(command, "!EW%c", signal+ASCII_0_TO_A); - send_recv_mess(card, command, warning_response); - Debug(2, "set_status, warning query, card %d, response=%s\n", card, warning_response); - - status.Bits.RA_PLUS_LS = 0; - status.Bits.RA_MINUS_LS = 0; - - /* The response string is an integer representation of a bit sequence */ - - int warning_code = atoi(warning_response); - int moving_bit = 9; - int moving = (warning_code & ( 1 << moving_bit )) >> moving_bit; - status.Bits.RA_DONE = moving ? 0 : 1; - Debug(2, "set_status, warning query, card %d, response=%d, moving=%d, done=%d\n", card, warning_code, moving, status.Bits.RA_DONE); - - /* Request the error state of the motor */ - sprintf(command, "!EE%c", signal+ASCII_0_TO_A); - send_recv_mess(card, command, error_response); - int error_code = atoi(error_response); - Debug(2, "set_status, error query, card %d, response=%d\n", card, error_code); - status.Bits.RA_PROBLEM = error_code > 0; - - /* encoder status */ - status.Bits.EA_SLIP = 0; - status.Bits.EA_POSITION = 0; - status.Bits.EA_SLIP_STALL = 0; - status.Bits.EA_HOME = 0; - - /* Request the position of this motor */ - sprintf(command, "!GP%c", signal+ASCII_0_TO_A); - send_recv_mess(card, command, position_response); - motorData = atoi(position_response); - Debug(2, "set_status, position query, card %d, response=%s\n", card, position_response); - - if (motorData == motor_info->position) - { - if (nodeptr != 0) /* Increment counter only if motor is moving. */ - motor_info->no_motion_count++; - } - else - { - status.Bits.RA_DIRECTION = (motorData >= motor_info->position) ? 1 : 0; - motor_info->position = motorData; - motor_info->encoder_position = motorData; - motor_info->no_motion_count = 0; - } - - rtn_state = (!motor_info->no_motion_count || - status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; - - /* Test for post-move string. */ - if (status.Bits.RA_DONE && nodeptr != 0 && - nodeptr->postmsgptr != 0) - { - strcpy(buff, nodeptr->postmsgptr); - strcat(buff, "\r"); - send_mess(card, buff, (char*) NULL); - nodeptr->postmsgptr = NULL; - } - - motor_info->status.All = status.All; - Debug(2, "set_status, return value=%d\n", rtn_state); - return (rtn_state); -} - -/********************************************************* - * Send a message to the LinMot board. - *********************************************************/ -STATIC RTN_STATUS send_mess(int card, const char *com, char *name) -{ - char temp[BUFF_SIZE]; - Debug(2, "send_mess: sending message via send_recv_mess to card %d, message=%s\n", card, com); - return (RTN_STATUS)send_recv_mess(card, com, temp); -} - -/********************************************************* - * Receive a message from the LinMot board. - *********************************************************/ -STATIC int recv_mess(int card, char *com, int flag) -{ - double timeout; - int flush; - asynStatus status; - size_t nread=0; - int eomReason; - struct LinMotController *cntrl; - - com[0] = '\0'; - /* Check that card exists */ - if (!motor_state[card]) - { - epicsPrintf("resv_mess - invalid card #%d\n", card); - return(-1); - } - - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - - if (flag == FLUSH) { - flush = 1; - timeout = 0; - } else { - flush = 0; - timeout = TIMEOUT; - } - if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser); - status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, - timeout, &nread, &eomReason); - - /* The response from the LinMot is terminated with CR/LF. Remove these */ - if (nread == 0) com[0] = '\0'; - if (nread > 0) { - /* Get rid of the preliminary # */ - memmove (com, com+1, strlen (com+1) + 1); - Debug(2, "recv_mess: card %d, flag=%d, message = \"%s\"\n", card, flag, com); - } - if (nread == 0) { - if (flag != FLUSH) { - Debug(1, "recv_mess: card %d read ERROR: no response\n", card); - } else { - Debug(3, "recv_mess: card %d flush returned no characters\n", card); - } - } - return(strlen(com)); -} - -/************************************************************ - * Send a message to the LinMot board and receive a resonse - ************************************************************/ -STATIC int send_recv_mess(int card, const char *out, char *response) -{ - char *p, *tok_save; - struct LinMotController *cntrl; - asynStatus status; - size_t nwrite=0, nread=0; - int eomReason; - char temp[BUFF_SIZE]; - - response[0] = '\0'; - - /* Check that card exists */ - if (!motor_state[card]) - { - epicsPrintf("send_recv_mess - invalid card #%d\n", card); - return (-1); - } - - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - - /* Device support can send us multiple commands separated with ';' - * characters. The LinMot cannot handle more than 1 command on a line - * so send them separately */ - strcpy(temp, out); - for (p = epicsStrtok_r(temp, ";", &tok_save); - ((p != NULL) && (strlen(p) != 0)); - p = epicsStrtok_r(NULL, ";", &tok_save)) { - Debug(2, "send_recv_mess: sending message to card %d, message=%s\n", card, p); - status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, p, strlen(p), - response, BUFF_SIZE, TIMEOUT, - &nwrite, &nread, &eomReason); - } - - /* The response from the LinMot is terminated with CR/LF. Remove these */ - if (nread == 0) response[0] = '\0'; - if (nread > 0) { - /* Get rid of the preliminary # */ - memmove(response, response+1, strlen (response+1) + 1); - Debug(2, "send_recv_mess: card %d, response=%s\n", card, response); - } - if (nread == 0) { - Debug(1, "send_recv_mess: card %d ERROR: no response\n", card); - } - return(strlen(response)); -} - -/************************************************************ - * Set up the LinMot motor - * Scan rat is in units of 1/60 seconds - ************************************************************/ -RTN_STATUS LinMotSetup(int num_cards, int scan_rate) -{ - int itera; - - if (num_cards < 1 || num_cards > LinMot_NUM_CARDS) - LinMot_num_cards = LinMot_NUM_CARDS; - else - LinMot_num_cards = num_cards; - - /* Set motor polling task rate */ - if (scan_rate >= 1 && scan_rate <= 60) - targs.motor_scan_rate = scan_rate; - else - targs.motor_scan_rate = SCAN_RATE; - - /* - * Allocate space for motor_state structure pointers. Note this must be done - * before LinMotConfig is called, so it cannot be done in motor_init() - * This means that we must allocate space for a card without knowing - * if it really exists, which is not a serious problem since this is just - * an array of pointers. - */ - motor_state = (struct controller **) malloc(LinMot_num_cards * - sizeof(struct controller *)); - - for (itera = 0; itera < LinMot_num_cards; itera++) - motor_state[itera] = (struct controller *) NULL; - return(OK); -} - -RTN_STATUS LinMotConfig(int card, /* Card being configured */ - const char *port, /* Asyn port name */ - int n_axes) /* Number of axes */ -{ - struct LinMotController *cntrl; - - if (card < 0 || card >= LinMot_num_cards) - return (ERROR); - - if (n_axes == 0) n_axes=1; /* This is a new parameter, some startup files don't have it yet */ - motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); - motor_state[card]->DevicePrivate = malloc(sizeof(struct LinMotController)); - cntrl = (struct LinMotController *) motor_state[card]->DevicePrivate; - cntrl->n_axes = n_axes; - /* Will be updated later but this is the default for a LinMot */ - cntrl->speed_resolution = 190735; - strcpy(cntrl->port, port); - return(OK); -} - -/************************************************************ - * Initialise motor software/hardware - ************************************************************/ -STATIC int motor_init() -{ - struct controller *brdptr; - struct LinMotController *cntrl; - int card_index, motor_index; - char response[BUFF_SIZE], command[BUFF_SIZE]; - int total_axis = 0; - int status; - bool success_rtn; - - initialized = true; /* Indicate that driver is initialized. */ - - /* Check for setup */ - if (LinMot_num_cards <= 0) - { - Debug(1, "motor_init: *LinMot driver disabled*\n"); - Debug(1, "LinMotSetup() is missing from startup script.\n"); - return (ERROR); - } - - for (card_index = 0; card_index < LinMot_num_cards; card_index++) - { - if (!motor_state[card_index]) - continue; - - brdptr = motor_state[card_index]; - total_cards = card_index + 1; - cntrl = (struct LinMotController *) brdptr->DevicePrivate; - - /* Initialize communications channel */ - success_rtn = false; - - status = pasynOctetSyncIO->connect(cntrl->port, 0, &cntrl->pasynUser, NULL); - success_rtn = (status == asynSuccess); - Debug(1, "motor_init, return from pasynOctetSyncIO->connect for port %s = %d, pasynUser=%p\n", cntrl->port, success_rtn, cntrl->pasynUser); - - if (success_rtn == true) - { - int retry = 0; - - /* Send a message to the board, see if it exists */ - /* flush any junk at input port - should not be any data available */ - do { - recv_mess(card_index, response, FLUSH); - } while (strlen(response) != 0); - - do - { - sprintf(command, "!VI%c;", card_index + ASCII_0_TO_A); - send_recv_mess(card_index, command, response); - retry++; - /* Return value is length of response string */ - } while(strlen(response) == 0 && retry < 3); - } - - if (success_rtn == true && strlen(response) > 0) - { - brdptr->localaddr = (char *) NULL; - brdptr->motor_in_motion = 0; - /* Leave bdptr->cmnd_response false because we read each response */ - /* in send_mess and send_recv_mess. */ - brdptr->cmnd_response = false; - - cntrl->speed_resolution = atoi(response); - total_axis = cntrl->n_axes; - brdptr->total_axis = total_axis; - start_status(card_index); - for (motor_index = 0; motor_index < total_axis; motor_index++) - { - struct mess_info *motor_info = &brdptr->motor_info[motor_index]; - - motor_info->motor_motion = NULL; - motor_info->status.All = 0; - motor_info->no_motion_count = 0; - motor_info->encoder_position = 0; - motor_info->position = 0; - set_status(card_index, motor_index); /* Read status of each motor */ - } - } - else { - motor_state[card_index] = (struct controller *) NULL; - } - } - - any_motor_in_motion = 0; - - Debug(3, "motor_init: spawning motor task\n"); - - mess_queue.head = (struct mess_node *) NULL; - mess_queue.tail = (struct mess_node *) NULL; - - free_list.head = (struct mess_node *) NULL; - free_list.tail = (struct mess_node *) NULL; - - epicsThreadCreate((char *) "tLinMot", epicsThreadPriorityMedium, - epicsThreadGetStackSize(epicsThreadStackMedium), - (EPICSTHREADFUNC) motor_task, (void *) &targs); - - return(OK); -} diff --git a/motorApp/LinMotSrc/drvLinMot.h b/motorApp/LinMotSrc/drvLinMot.h deleted file mode 100644 index 85ffab08a..000000000 --- a/motorApp/LinMotSrc/drvLinMot.h +++ /dev/null @@ -1,44 +0,0 @@ -/* File: drvLinMot.h */ -/* Version: 2.0 */ -/* Date Last Modified: 09-29-99 */ - - -/* Device Driver Support definitions for motor */ -/* - * Original Author: Mark Rivers - * Current Author: Mark Rivers - * Date: 11/20/98 - * - * Modification Log: - * ----------------- - * .01 11/20/98 mlr initialized from drvMM4000.h - * .02 09/29/99 mlr re-wrote for new version of motor software (V4) - * .03 02/11/03 mlr Added support for PM600 model - */ - -#ifndef INCdrvLinMoth -#define INCdrvLinMoth 1 - -#include "motordrvCom.h" -#include "asynOctetSyncIO.h" - -/* LinMot default profile. */ - -#define LinMot_NUM_CARDS 4 -#define LinMot_MAX_CHANNELS 10 - -#define MODEL_LinMot 0 -#define MODEL_PM600 1 - -struct LinMotController -{ - asynUser *pasynUser; /* asyn */ - int n_axes; /* Number of axes on this controller */ - char port[80]; /* asyn port name */ - int speed_resolution; /* The motor speed resolution. Default set to LinMot standard */ -}; - -RTN_STATUS LinMotSetup(int, int); -RTN_STATUS LinMotConfig(int, const char *, int); - -#endif /* INCdrvLinMoth */ diff --git a/motorApp/SM300Src/Makefile b/motorApp/SM300Src/Makefile deleted file mode 100644 index d49593bad..000000000 --- a/motorApp/SM300Src/Makefile +++ /dev/null @@ -1,30 +0,0 @@ -# Makefile -TOP=../.. - -include $(TOP)/configure/CONFIG -#---------------------------------------- -# ADD MACRO DEFINITIONS AFTER THIS LINE -#============================= - -#================================================== -# Build an IOC support library -LIBRARY_IOC = SM300Motor - -# motorRecord.h will be created from motorRecord.dbd -# install devMotorSoft.dbd into /dbd -DBD += SM300Motor.dbd - -# The following are compiled and added to the Support library -SRCS += SM300Register.cc - -SRCS += SM300Driver.cpp - - -SM300Motor_LIBS += motor -SM300Motor_LIBS += asyn -SM300Motor_LIBS += $(EPICS_BASE_IOC_LIBS) - -include $(TOP)/configure/RULES -#---------------------------------------- -# ADD RULES AFTER THIS LINE - diff --git a/motorApp/SM300Src/SM300Driver.cpp b/motorApp/SM300Src/SM300Driver.cpp deleted file mode 100644 index 82c20fd79..000000000 --- a/motorApp/SM300Src/SM300Driver.cpp +++ /dev/null @@ -1,599 +0,0 @@ -/* -Motor driver support for the SM300 controller. - -Based on the SM100 Model 3 device driver -*/ - - -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include "asynMotorController.h" -#include "asynMotorAxis.h" - -#include -#include -#include "SM300Driver.h" - -/** Creates a new SM300Controller object. - * \param[in] portName The name of the asyn port that will be created for this driver - * \param[in] SM300PortName The name of the drvAsynSerialPort that was created previously to connect to the SM300 controller - * \param[in] numAxes The number of axes that this controller supports - * \param[in] movingPollPeriod The time between polls when any axis is moving - * \param[in] idlePollPeriod The time between polls when no axis is moving - */ -SM300Controller::SM300Controller(const char *portName, const char *SM300PortName, int numAxes, - double movingPollPeriod, double idlePollPeriod) - : asynMotorController(portName, numAxes, NUM_SM300_PARAMS, - 0, // No additional interfaces beyond those in base class - 0, // No additional callback interfaces beyond those in base class - ASYN_CANBLOCK | ASYN_MULTIDEVICE, - 1, // autoconnect - 0, 0), // Default priority and stack size - is_moving_(false), - polls_count_(0) -{ - asynStatus status; - static const char *functionName = "SM300Controller::SM300Controller"; - - /* Connect to SM300 controller */ - status = pasynOctetSyncIO->connect(SM300PortName, 0, &pasynUserController_, NULL); - if (status) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: cannot connect to SM300 controller\n", - functionName); - } - if (numAxes != 2) { - errlogPrintf("SM300: Driver is only setup for two axes X and Y!\n"); - } - new SM300Axis(this, 0, 'X'); - new SM300Axis(this, 1, 'Y'); - - createParam(SM300ResetString, asynParamInt32, &reset_); - createParam(SM300ResetAndHomeString, asynParamInt32, &reset_and_home_); - createParam(SM300DisconnectString, asynParamInt32, &disconnect_); - createParam(SM300ErrorCodeString, asynParamInt32, &error_code_); - - startPoller(movingPollPeriod, idlePollPeriod, 2); -} - -/** Send a query string to the controller and get a return value. - * query string is prefeced with ACK STX and postfixed with EOT - * return string ends with ETX - * \param[in] query the query to send - * \param[in] what form the query reply has; true if the reply has EOT false for ETX with BCC - * \returns success of write and read of query string - */ -asynStatus SM300Controller::sendQuery(const char * query, bool hasEotEnding) { - if (hasEotEnding) { - setTerminationChars("\x04", 1, "\x04", 1); - } - else { - setTerminationChars("\x03", 1, "\x04", 1); - } - //send data format 2 - sprintf(this->outString_, "\x06\x02%s", query); - return this->writeReadController(); -} - -/** Send a command string to the controller. -* Command string is prefeced with ACK STX and postfixed with EOT -* \param[in] command the command to send -* \returns success of write and read of acknowledgement from controller -*/ -asynStatus SM300Controller::sendCommand(const char * command) { - setTerminationChars("\x06", 1, "\x04", 1); - //send data format 2 - sprintf(this->outString_, "\x06\x02%s", command); - return this->writeReadController(); -} - -/** - * Return true if the controller registering moving motors - */ -bool SM300Controller::is_moving() { - return is_moving_; -} - -/** - * deal with db records being set which are integers - * \returns status - */ -asynStatus SM300Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { - asynStatus status; - - int function = pasynUser->reason; //Function requested - if (function == reset_) { - if (value == 0) return asynSuccess; - status = perform_reset(); - callParamCallbacks(); - } - else if (function == disconnect_) { - setIntegerParam(disconnect_, 1); - callParamCallbacks(); - setTerminationChars("\x06", 1, "\x04", 1); - sprintf(this->outString_, "\x06\x02%s", "M77"); - status = writeController(); - setIntegerParam(disconnect_, 0); - callParamCallbacks(); - } - else if (function == reset_and_home_) { - setIntegerParam(reset_and_home_, 1); - callParamCallbacks(); - status = perform_reset(); - for (int i = 0; i < numAxes_; i++) { - getAxis(i)->home(0, 0, 0, 0); - } - setIntegerParam(reset_and_home_, 0); - callParamCallbacks(); - } - else { - status = asynMotorController::writeInt32(pasynUser, value); - } - return status; -} - -/** - * Perform a reset of the controller sending it initiation parameters - * \returns status -*/ -asynStatus SM300Controller::perform_reset() { - setIntegerParam(reset_, 1); - callParamCallbacks(); - setTerminationChars("\x04", 1, "\x04", 1); - - //send empty string with ACK to clear the buffer - asynStatus status = this->writeController(); - if (status) return status; - - //set termination character is EOT without check sum * - // when sending send incase this mode is switched on - setTerminationChars("\x06", 1, "\x04\x0D", 2); - if ( (status = sendCommand("PEK0")) ) return status; - - setTerminationChars("\x06", 1, "\x04", 1); - const char * commands[] = { - // set achknowlegement on - "PEL1", - // set linear interpolation - "B/ G01", - // absoulute coordinates - "B/ G90", - // switch off message from contrl unit when motor is in position or in error (will poll instead) - "PER0", - //send data format 2 - 100s - "PXA2", - "PYA2", - // Gear factor numerator - "PXB5", - "PYB1", - // Gear factor denomenator - "PXC10", - "PYC10", - // Drag Error - "PXD2500", - "PYD2500", - // Start/stop ramp - "PXE100000", - "PYE25000", - // KV factor oe feedback control amplification - "PXF1000", - "PYF1000", - // Regulator factor A - "PXG0", - "PYG0", - // +Software switch limit - "PXH+57000", - "PYH+64000", - // -Software switch limit - "PXI-50", - "PYI-20", - // maximum speed - "PXJ25000", - "PYJ25000", - // direction and speed of homing procedure - "PXK-2500", - "PYK-7500", - // standstill check - "PXL100", - "PYL100", - // Inposition window - "PXM5", - "PYM5", - // Distance from reference switch - "PXN1000", - "PYN5000", - // Backlash compensation - "PXO0", - "PYO0", - // Moving direction - "PXP0", - "PYP0", - // Feed for axes - "BF15000" - }; - - int n_array = (sizeof(commands) / sizeof(const char *)); - for (int i = 0; i < n_array; i++) { - if ( (status = sendCommand(commands[i])) ) return status; - } - - setIntegerParam(reset_, 0); - return asynSuccess; -} - - -/** Creates a new SM300Controller object. - * Configuration command, called directly or from iocsh - * \param[in] portName The name of the asyn port that will be created for this driver - * \param[in] SM300PortName The name of the drvAsynIPPPort that was created previously to connect to the SM300 controller - * \param[in] numAxes The number of axes that this controller supports - * \param[in] movingPollPeriod The time in ms between polls when any axis is moving - * \param[in] idlePollPeriod The time in ms between polls when no axis is moving - * \param[in] eguPerStep The stage resolution - */ -extern "C" int SM300CreateController(const char *portName, const char *SM300PortName, int numAxes, - int movingPollPeriod, int idlePollPeriod) -{ - new SM300Controller(portName, SM300PortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.); - //printf("\n *** SM300: stepSize=%f\n", stepSize); - - return(asynSuccess); -} - -/** Reports on status of the driver - * \param[in] fp The file pointer on which report information will be written - * \param[in] level The level of report detail desired - * - * If details > 0 then information is printed about each axis. - * After printing controller-specific information it calls asynMotorController::report() - */ -void SM300Controller::report(FILE *fp, int level) -{ - fprintf(fp, "SM300 motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", - this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_); - - // Call the base class method - asynMotorController::report(fp, level); -} - -/** Returns a pointer to an SM300Axis object. - * Returns NULL if the axis number encoded in pasynUser is invalid. - * \param[in] pasynUser asynUser structure that encodes the axis index number. */ -SM300Axis* SM300Controller::getAxis(asynUser *pasynUser) -{ - return static_cast(asynMotorController::getAxis(pasynUser)); -} - -/** Returns a pointer to an SM300Axis object. - * Returns NULL if the axis number encoded in pasynUser is invalid. - * \param[in] No Axis index number. */ -SM300Axis* SM300Controller::getAxis(int axisNo) -{ - return static_cast(asynMotorController::getAxis(axisNo)); -} - -/** Set the termination characters on the output and input buffers - * \param[in] eosIn end of string for input - * \param[in] eosInlen length of end of string for input - * \param[in] eosOut end of string for output - * \param[in] eosOutlen length of end of string for output - */ -void SM300Controller::setTerminationChars(const char *eosIn, int eosInlen, const char *eosOut, int eosOutlen) { - - pasynOctetSyncIO->setOutputEos(this->pasynUserController_, eosOut, eosOutlen); - pasynOctetSyncIO->setInputEos(this->pasynUserController_, eosIn, eosInlen); -} - -/** Polls the axis. - * This function reads moving status which is done on a controller - */ -asynStatus SM300Controller::poll() -{ - asynStatus comStatus; - SM300Axis *axis; - bool motorHasError = false; - int code; - - // Read the current status of the motor (at Poition, Not at position, Error) - comStatus = this->sendQuery("LM", true); - if (comStatus) goto skip; - - if (strlen(this->inString_) < 3) { - comStatus = asynError; - errlogPrintf("SM300 poll: moving status return string is too short.\n"); - goto skip; - } - - if (this->inString_[2] == 'P') { // axis in Position - is_moving_ = false; - - // Finished a home - if (axis_x_homing_) { - axis_x_homing_ = false; - home_axis_x_ = false; - } - if (axis_y_homing_) { - axis_y_homing_ = false; - home_axis_y_ = false; - } - // If need to home to now - if (home_axis_x_) { - comStatus = sendCommand("BRX"); - axis_x_homing_ = true; - if (comStatus) { - errlogPrintf("SM300 poll: Failed to home x.\n"); - } - } else if (home_axis_y_) { - comStatus = sendCommand("BRY"); - axis_y_homing_ = true; - if (comStatus) { - errlogPrintf("SM300 poll: Failed to home y.\n"); - } - } - } - else if (this->inString_[2] == 'N') { // Not in position - is_moving_ = true; - } - else { - motorHasError = true; - is_moving_ = false; - comStatus = asynError; - errlogPrintf("SM300 poll: moving status returned errors status.\n"); - } - - for (int i=0; i < this->numAxes_; i++) { - axis = this->getAxis(i); - axis->setIntegerParam(motorStatusDone_, is_moving_ ? 0 : 1); - } - - comStatus = sendQuery("LS10", false); - if (comStatus) goto skip; - - if (strlen(this->inString_) < 3) { - comStatus = asynError; - errlogPrintf("SM300 error code: return string is too short.\n"); - goto skip; - } - code = strtol(&this->inString_[2], NULL, 16); - - if (code != 0) { - motorHasError = true; - } - - // special case for error in sending command - if (code >= 0xF && code <= 0x14) { - errlogPrintf("SM300 error code: CNC cmd error code, code = %.2x\n", code); - code = 0xF; - } - // speacial case for error in CNC interpreter - if (code >= 0x1E && code <= 0x4F) { - errlogPrintf("SM300 error code: CNC cmd error, code = %.2x\n", code); - code = 0xE; - } - - setIntegerParam(error_code_, code); - -skip: - has_error_ = comStatus != asynSuccess; - - for (int i = 0; i < this->numAxes_; i++) { - axis = this->getAxis(i); - // make sure status is set for refresh during first 10 polls otherwise it is set on start - if (polls_count_ < REFRESH_ERROR_FOR_POLL_COUNTS) { - axis->setIntegerParam(this->motorStatusCommsError_, axis->has_error() || has_error_ ? 0 : 1); - polls_count_++; - } - axis->setIntegerParam(this->motorStatusCommsError_, axis->has_error() || has_error_ ? 1 : 0); - axis->setIntegerParam(this->motorStatusProblem_, motorHasError ? 1 : 0); - axis->callParamCallbacks(); - } - - callParamCallbacks(); - return comStatus ? asynError : asynSuccess; -} - -/** Get an whether the controller has an error. -*/ -bool SM300Controller::has_error() { - return has_error_; -} - - -// These are the SM300Axis methods - -/** Creates a new SM300Axis object. - * \param[in] pC Pointer to the SM300Controller to which this axis belongs. - * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. - * - * Initializes register numbers, etc. - */ -SM300Axis::SM300Axis(SM300Controller *pC, int axisNo, char axisLabel) - : asynMotorAxis(pC, axisNo), - pC_(pC), axisLabel(axisLabel) -{ -} - -/** Get an whether the axis has an error. -*/ -bool SM300Axis::has_error() { - return has_error_; -} - -/** Polls the axis. -* This function reads the position of the axis -* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). -* \returns success of poll -*/ -asynStatus SM300Axis::poll(bool *moving) -{ - char temp[MAX_CONTROLLER_STRING_SIZE]; - double position; - asynStatus comStatus; - - *moving = pC_->is_moving(); - - // Read the current motor position - sprintf(temp, "LI%c", this->axisLabel); - comStatus = pC_->sendQuery(temp, false); - if (comStatus) goto skip; - - // The response string is of the form "\06\02%d" - if (strlen(pC_->inString_) < 3) { - comStatus = asynError; - errlogPrintf("SM300 poll: position return string is too short.\n"); - goto skip; - } - - char *stop_char; - position = (strtod(&pC_->inString_[2], &stop_char) / 1.0); - if (stop_char[0] != '\0') { - errlogPrintf("SM300 poll: LI reply contains non number %s.\n", &pC_->inString_[2]); - comStatus = asynError; - goto skip; - } - setDoubleParam(pC_->motorPosition_, position); - -skip: - has_error_ = comStatus != asynSuccess; - setIntegerParam(pC_->motorStatusCommsError_, has_error_ || pC_->has_error() ? 1 : 0); - callParamCallbacks(); - return comStatus ? asynError : asynSuccess; -} - - -/** Reports on status of the axis - * \param[in] fp The file pointer on which report information will be written - * \param[in] level The level of report detail desired - * - * After printing device-specific information calls asynMotorAxis::report() - */ -void SM300Axis::report(FILE *fp, int level) -{ - if (level > 0) { - fprintf(fp, " axis %d\n", axisNo_ + 1); - } - - // Call the base class method - asynMotorAxis::report(fp, level); -} - - -/** Move the motor to an absolute location or by a relative amount (uses current location since the motor doesw not support a relative command). -* \param[in] position The absolute position to move to (if relative=0) or the relative distance to move -* by (if relative=1). Units=steps. -* \param[in] relative Flag indicating relative move (1) or absolute move (0). -* \param[in] minVelocity The initial velocity, often called the base velocity. Units=steps/sec. -* \param[in] maxVelocity The maximum velocity, often called the slew velocity. Units=steps/sec. -* \param[in] acceleration The acceleration value. Units=steps/sec/sec. */ -asynStatus SM300Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) -{ - asynStatus comStatus; - char temp[MAX_CONTROLLER_STRING_SIZE]; - double move_to = position; - - //status = sendAccelAndVelocity(acceleration, maxVelocity); - - if (relative==1) { //relative move - errlogPrintf("SM300 move: Can not do relative move with this motor.\n"); - return asynError; - } - - sprintf(temp, "B%c%.0f", axisLabel, floor(move_to + 0.5)); - comStatus = pC_->sendCommand(temp); - - if (comStatus) goto skip; - comStatus = pC_->sendCommand("BSL"); - -skip: - return comStatus; - -} - -/** Home an axis - * - * This stores the intention to home when the motor comes to a stop. The reason it is done this way is that if the motors is homing - * one axis it can not home any other axis. It wil home the next time the motor is stationary. - * /param[in] axis the axis to home (X or Y) -*/ -void SM300Controller::homeAxis(const char axis) { - if (axis == 'X') { - if (!axis_x_homing_) { - home_axis_x_ = true; - } - } - if (axis == 'Y') { - if (!axis_y_homing_) { - home_axis_y_ = true; - } - } -} - -/** Home the axis - * velocities and accelerations are ignored these are set by the controller. - */ -asynStatus SM300Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) -{ - pC_->homeAxis(axisLabel); - return asynSuccess; -} - -// Jog -asynStatus SM300Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) -{ - errlogPrintf("Device does not support move velocity (jog)"); - return asynError; - -} - -/** Stop of motor axis moving (Stops all axes) - * \returns send command status -*/ -asynStatus SM300Axis::stop(double acceleration ) -{ - // Motor can not be stopped using BSS because if it is then it immediately sets the position to 0 wherever the motor stops. - return asynSuccess; -} - -/** Set absolute position in hardware - * Not supported - */ -asynStatus SM300Axis::setPosition(double position) -{ - errlogPrintf("Device does not support set position"); - return asynError; -} - -/** Code for iocsh registration */ -static const iocshArg SM300CreateControllerArg0 = {"Port name", iocshArgString}; -static const iocshArg SM300CreateControllerArg1 = {"SM300 port name", iocshArgString}; -static const iocshArg SM300CreateControllerArg2 = {"Number of axes", iocshArgInt}; -static const iocshArg SM300CreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt}; -static const iocshArg SM300CreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt}; -static const iocshArg * const SM300CreateControllerArgs[] = {&SM300CreateControllerArg0, - &SM300CreateControllerArg1, - &SM300CreateControllerArg2, - &SM300CreateControllerArg3, - &SM300CreateControllerArg4}; -static const iocshFuncDef SM300CreateControllerDef = {"SM300CreateController", 5, SM300CreateControllerArgs}; -static void SM300CreateContollerCallFunc(const iocshArgBuf *args) -{ - SM300CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival); -} - -static void SM300Register(void) -{ - iocshRegister(&SM300CreateControllerDef, SM300CreateContollerCallFunc); -} - -extern "C" { -epicsExportRegistrar(SM300Register); -} diff --git a/motorApp/SM300Src/SM300Driver.h b/motorApp/SM300Src/SM300Driver.h deleted file mode 100644 index 2f7dc8ca6..000000000 --- a/motorApp/SM300Src/SM300Driver.h +++ /dev/null @@ -1,81 +0,0 @@ -/* -Motor driver support for the SM300 controller. -*/ - -#ifndef SM300Driver_H -#define SM300Driver_H - -// controller-specific parameters yet -#define SM300ResetString "RESET" -#define SM300ResetAndHomeString "RESET_AND_HOME" -#define SM300DisconnectString "DISCONNECT" -#define SM300ErrorCodeString "ERROR_CODE" - -class epicsShareClass SM300Axis : public asynMotorAxis -{ -public: - /* These are the methods we override from the base class */ - SM300Axis(class SM300Controller *pC, int axis, char axisLabel); - void report(FILE *fp, int level); - 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); - bool has_error(); -private: - SM300Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. - * Abbreviated because it is used very frequently */ - double stepSize_; /**< Encoder increment value obtained with SU? command _or_ resolution, set at boot time */ - /* with SMC100CreateController command */ - char axisLabel; /** label for the axis*/ - bool has_error_; -friend class SM300Controller; -}; - -class SM300Controller : public asynMotorController { -public: - SM300Controller(const char *portName, const char *SMC100PortName, int numAxes, double movingPollPeriod, double idlePollPeriod); - void report(FILE *fp, int level); - SM300Axis* getAxis(asynUser *pasynUser); - SM300Axis* getAxis(int axisNo); - void setTerminationChars(const char *eosIn, int eosInlen, const char *eosOut, int eosOutlen); - - virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); - - virtual asynStatus poll(); - asynStatus sendCommand(const char * query); - asynStatus sendQuery(const char * query, bool hasEotEnding); - bool has_error(); - bool is_moving(); - void homeAxis(const char axis); -private: - asynStatus perform_reset(); - - bool has_error_; - bool is_moving_; - bool axis_x_homing_; - bool axis_y_homing_; - bool home_axis_x_; - bool home_axis_y_; - - int polls_count_; - - // number of times the error flag should be refreshed manually. 10 is arbitary bt works. - static const int REFRESH_ERROR_FOR_POLL_COUNTS = 10; - - #define FIRST_SM300_PARAM reset_ - int reset_; // int param - int reset_and_home_; // int param - int disconnect_; // int param - int error_code_; // int param - #define LAST_SM300_PARAM error_code_ - - -friend class SM300Axis; - -}; - -#define NUM_SM300_PARAMS (&LAST_SM300_PARAM - &FIRST_SM300_PARAM + 1) -#endif // SM300Driver_H diff --git a/motorApp/SM300Src/SM300Motor.dbd b/motorApp/SM300Src/SM300Motor.dbd deleted file mode 100644 index 39ed28453..000000000 --- a/motorApp/SM300Src/SM300Motor.dbd +++ /dev/null @@ -1,3 +0,0 @@ -# SM300 Device Driver support. -registrar(SM300Register) - diff --git a/motorApp/SM300Src/SM300Register.cc b/motorApp/SM300Src/SM300Register.cc deleted file mode 100644 index a98446fa2..000000000 --- a/motorApp/SM300Src/SM300Register.cc +++ /dev/null @@ -1,41 +0,0 @@ -/* -FILENAME... SM300Register.cc -USAGE... Register SM300 motor device driver shell commands. - -Version: $Revision: 1.4 $ -Modified By: $Author: sluiter $ -Last Modified: $Date: 2004-07-16 19:06:58 $ -*/ - -/***************************************************************** - COPYRIGHT NOTIFICATION -***************************************************************** - -(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO - -This software was developed under a United States Government license -described on the COPYRIGHT_UniversityOfChicago file included as part -of this distribution. -**********************************************************************/ - -#include -#include "SM300Register.h" -#include "epicsExport.h" - -extern "C" -{ - -// SM300 Setup arguments -static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; -static const iocshArg setupArg1 = {"Polling rate", iocshArgInt}; -// SM300 Config arguments -static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; -static const iocshArg configArg1 = {"asyn port name", iocshArgString}; - -static const iocshArg * const SM300SetupArgs[2] = {&setupArg0, &setupArg1}; -static const iocshArg * const SM300ConfigArgs[2] = {&configArg0, &configArg1}; - -static const iocshFuncDef setupSM300 = {"SM300Setup", 2, SM300SetupArgs}; -static const iocshFuncDef configSM300 = {"SM300Config", 2, SM300ConfigArgs}; - -} // extern "C" diff --git a/motorApp/SM300Src/SM300Register.h b/motorApp/SM300Src/SM300Register.h deleted file mode 100644 index 3cbe329f0..000000000 --- a/motorApp/SM300Src/SM300Register.h +++ /dev/null @@ -1,12 +0,0 @@ -/* -This file contains function prototypes for SM300 IOC shell commands. - -*/ - -#include "motor.h" -#include "motordrvCom.h" - -/* Function prototypes. */ -extern RTN_STATUS SM300Setup(int, int); -extern RTN_STATUS SM300Config(int, const char *); - From f2757e692b43411277cf15de4aa5b134466654eb Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 8 Jul 2025 23:05:40 +0100 Subject: [PATCH 226/232] Remove iocBoot --- iocBoot/iocmotorSim/Makefile | 14 ---- iocBoot/iocmotorSim/README | 24 ------- iocBoot/iocmotorSim/auto_positions.req | 8 --- iocBoot/iocmotorSim/auto_settings.req | 11 --- iocBoot/iocmotorSim/config.xml | 6 -- iocBoot/iocmotorSim/motor.substitutions | 14 ---- .../iocmotorSim/motor.substitutions.allstop | 14 ---- iocBoot/iocmotorSim/saveData.req | 43 ------------ iocBoot/iocmotorSim/save_restore.cmd | 68 ------------------- iocBoot/iocmotorSim/st.cmd.Vx | 27 -------- iocBoot/iocmotorSim/st.cmd.allstop | 56 --------------- iocBoot/iocmotorSim/st.cmd.motorsim | 49 ------------- iocBoot/iocmotorSim/st.cmd.sim.inst | 28 -------- iocBoot/iocmotorSim/st.cmd.unix | 27 -------- iocBoot/iocmotorSim/st.cmd.win32 | 14 ---- iocBoot/iocmotorSim/start_medm | 1 - 16 files changed, 404 deletions(-) delete mode 100755 iocBoot/iocmotorSim/Makefile delete mode 100644 iocBoot/iocmotorSim/README delete mode 100644 iocBoot/iocmotorSim/auto_positions.req delete mode 100644 iocBoot/iocmotorSim/auto_settings.req delete mode 100644 iocBoot/iocmotorSim/config.xml delete mode 100644 iocBoot/iocmotorSim/motor.substitutions delete mode 100644 iocBoot/iocmotorSim/motor.substitutions.allstop delete mode 100644 iocBoot/iocmotorSim/saveData.req delete mode 100644 iocBoot/iocmotorSim/save_restore.cmd delete mode 100644 iocBoot/iocmotorSim/st.cmd.Vx delete mode 100644 iocBoot/iocmotorSim/st.cmd.allstop delete mode 100644 iocBoot/iocmotorSim/st.cmd.motorsim delete mode 100644 iocBoot/iocmotorSim/st.cmd.sim.inst delete mode 100644 iocBoot/iocmotorSim/st.cmd.unix delete mode 100755 iocBoot/iocmotorSim/st.cmd.win32 delete mode 100755 iocBoot/iocmotorSim/start_medm diff --git a/iocBoot/iocmotorSim/Makefile b/iocBoot/iocmotorSim/Makefile deleted file mode 100755 index 2ca9fa11f..000000000 --- a/iocBoot/iocmotorSim/Makefile +++ /dev/null @@ -1,14 +0,0 @@ -TOP = ../.. -include $(TOP)/configure/CONFIG -ARCH = $(EPICS_HOST_ARCH) -#ARCH = linux-x86_64 -#ARCH = vxWorks-ppc604 -TARGETS = cdCommands -TARGETS += envPaths dllPath.bat runIOC.bat relPaths.sh runIOC.sh -TARGETS += motor.substitutions.local - -motor.substitutions.local : motor.substitutions - $(MSI) -M "IOC=$(MYPVPREFIX)" -o $@ $< - -include $(TOP)/configure/RULES.ioc - diff --git a/iocBoot/iocmotorSim/README b/iocBoot/iocmotorSim/README deleted file mode 100644 index f7ddad1ad..000000000 --- a/iocBoot/iocmotorSim/README +++ /dev/null @@ -1,24 +0,0 @@ -To build any examples; - -- in /configure/RELEASE: EPICS_BASE and MOTOR must be defined. - -- in /Makefile: the following three lines must be uncommented; - #!DIRS += motorExApp iocBoot - #!motorExApp_DEPEND_DIRS = motorApp - #!iocBoot_DEPEND_DIRS = motorExApp - -To build this simulation example; - -- in /configure/RELEASE: ASYN must be defined. - -- in /iocBoot/iocSim/Makefile set the target architecture - -Finally, cd ; gnumake clean uninstall; gnumake - -To run this simulation example on a Unix OS; -- Set the EPICS_HOST_ARCH environment variable correctly. -- Edit the st.cmd.unix file for either sun or linux. -- Start the ioc from this directory by executing the following command. - -../../bin/${EPICS_HOST_ARCH}/WithAsyn st.cmd.unix - diff --git a/iocBoot/iocmotorSim/auto_positions.req b/iocBoot/iocmotorSim/auto_positions.req deleted file mode 100644 index 64e804c38..000000000 --- a/iocBoot/iocmotorSim/auto_positions.req +++ /dev/null @@ -1,8 +0,0 @@ -$(P)m1.DVAL -$(P)m2.DVAL -$(P)m3.DVAL -$(P)m4.DVAL -$(P)m5.DVAL -$(P)m6.DVAL -$(P)m7.DVAL -$(P)m8.DVAL diff --git a/iocBoot/iocmotorSim/auto_settings.req b/iocBoot/iocmotorSim/auto_settings.req deleted file mode 100644 index 64d745c47..000000000 --- a/iocBoot/iocmotorSim/auto_settings.req +++ /dev/null @@ -1,11 +0,0 @@ -$(P)saveData_config - -file motor_settings.req P=$(P),M=m1 -file motor_settings.req P=$(P),M=m2 -file motor_settings.req P=$(P),M=m3 -file motor_settings.req P=$(P),M=m4 -file motor_settings.req P=$(P),M=m5 -file motor_settings.req P=$(P),M=m6 -file motor_settings.req P=$(P),M=m7 -file motor_settings.req P=$(P),M=m8 - diff --git a/iocBoot/iocmotorSim/config.xml b/iocBoot/iocmotorSim/config.xml deleted file mode 100644 index 32b076052..000000000 --- a/iocBoot/iocmotorSim/config.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - -Simulated motor - - diff --git a/iocBoot/iocmotorSim/motor.substitutions b/iocBoot/iocmotorSim/motor.substitutions deleted file mode 100644 index 44acf3759..000000000 --- a/iocBoot/iocmotorSim/motor.substitutions +++ /dev/null @@ -1,14 +0,0 @@ -file "../../db/basic_asyn_motor.db" -{ -pattern -{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT} -{"$(IOC)", 1, "m$(N)", "asynMotor", motorSim1, 0, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{"$(IOC)", 2, "m$(N)", "asynMotor", motorSim1, 1, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{"$(IOC)", 3, "m$(N)", "asynMotor", motorSim1, 2, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{"$(IOC)", 4, "m$(N)", "asynMotor", motorSim1, 3, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} - -{"$(IOC)", 5, "m$(N)", "asynMotor", motorSim2, 0, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{"$(IOC)", 6, "m$(N)", "asynMotor", motorSim2, 1, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{"$(IOC)", 7, "m$(N)", "asynMotor", motorSim2, 2, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -{"$(IOC)", 8, "m$(N)", "asynMotor", motorSim2, 3, "motor $(N)", degrees, Pos, 1, .1, .2, 0, 1, .2, 0.01, 5, 100, -100, ""} -} diff --git a/iocBoot/iocmotorSim/motor.substitutions.allstop b/iocBoot/iocmotorSim/motor.substitutions.allstop deleted file mode 100644 index 1ebd42ad8..000000000 --- a/iocBoot/iocmotorSim/motor.substitutions.allstop +++ /dev/null @@ -1,14 +0,0 @@ -file "../../db/stop_all.db" -{ -pattern -{P, N, M, AS} -{"NDW1298:sjb99183:", 1, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 2, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 3, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 4, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} - -{"NDW1298:sjb99183:", 5, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 6, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 7, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -{"NDW1298:sjb99183:", 8, "MOT:MTR060$(N,undefined)", NDW1298:sjb99183:CS} -} diff --git a/iocBoot/iocmotorSim/saveData.req b/iocBoot/iocmotorSim/saveData.req deleted file mode 100644 index cdb157f90..000000000 --- a/iocBoot/iocmotorSim/saveData.req +++ /dev/null @@ -1,43 +0,0 @@ -[prefix] -$(P) - -[status] -$(P)saveData_status - -[message] -$(P)saveData_message - -[filename] -$(P)saveData_fileName - -[counter] # scan counter -$(P)saveData_scanNumber - -[fileSystem] # scan file system -$(P)saveData_fileSystem - -[subdir] # scan file subdirectory -$(P)saveData_subDir - -[basename] # PV containing the desired data-file base name -$(P)saveData_baseName - -[fullPathName] -$(P)saveData_fullPathName - -[realTime1D] # if nonzero, write 1D data as it comes in -$(P)saveData_realTime1D - -[scanRecord] # specify scan records to be monitored -$(P)scanH -$(P)scan1 -$(P)scan2 -$(P)scan3 -$(P)scan4 - -[extraPV] -$(P)userCalc1.CALC "string" -$(P)saveData_realTime1D "enum" -$(P)saveData_scanNumber "long" -$(P)saveData_fileSystem "filesystem" -$(P)scan1.P1SM "scan mode" diff --git a/iocBoot/iocmotorSim/save_restore.cmd b/iocBoot/iocmotorSim/save_restore.cmd deleted file mode 100644 index b6b9e7010..000000000 --- a/iocBoot/iocmotorSim/save_restore.cmd +++ /dev/null @@ -1,68 +0,0 @@ - -# BEGIN save_restore.cmd ------------------------------------------------------ - -### save_restore setup -# -# This file does not require modification for standard use, but... - -# status PVs -#save_restoreSet_UseStatusPVs(1) -save_restoreSet_status_prefix("$(MYPVPREFIX)") -dbLoadRecords("$(AUTOSAVE)/asApp/Db/save_restoreStatus.db", "P=$(MYPVPREFIX), DEAD_SECONDS=5") - -# Ok to save/restore save sets with missing values (no CA connection to PV)? -save_restoreSet_IncompleteSetsOk(1) - -# Save dated backup files? -save_restoreSet_DatedBackupFiles(1) - -# Number of sequenced backup files to write -save_restoreSet_NumSeqFiles(3) - -# Time interval between sequenced backups -save_restoreSet_SeqPeriodInSeconds(300) - -# specify where save files should be -set_savefile_path("$(TOP)/iocBoot/$(IOC)", "autosave") - -### -# specify what save files should be restored. Note these files must be -# in the directory specified in set_savefile_path(), or, if that function -# has not been called, from the directory current when iocInit is invoked -set_pass0_restoreFile("auto_positions.sav") -set_pass0_restoreFile("auto_settings.sav") -set_pass1_restoreFile("auto_settings.sav") - -# Note that you can restore a .sav file without also autosaving to it. -#set_pass0_restoreFile("myInitData.sav") -#set_pass1_restoreFile("myInitData.sav") - -### -# specify directories in which to to search for included request files -set_requestfile_path("$(TOP)/iocBoot/$(IOC)", "") -set_requestfile_path("$(TOP)/iocBoot/$(IOC)", "autosave") -set_requestfile_path("$(AREA_DETECTOR)", "ADApp/Db") -set_requestfile_path("$(AUTOSAVE)", "asApp/Db") -set_requestfile_path("$(CALC)", "calcApp/Db") -set_requestfile_path("$(CAMAC)", "camacApp/Db") -set_requestfile_path("$(DAC128V)", "dac128VApp/Db") -set_requestfile_path("$(DXP)", "dxpApp/Db") -set_requestfile_path("$(IP)", "ipApp/Db") -set_requestfile_path("$(IP330)", "ip330App/Db") -set_requestfile_path("$(IPUNIDIG)", "ipUnidigApp/Db") -set_requestfile_path("$(LOVE)", "loveApp/Db") -set_requestfile_path("$(MCA)", "mcaApp/Db") -set_requestfile_path("$(MODBUS)", "modbusApp/Db") -set_requestfile_path("$(MOTOR)", "motorApp/Db") -set_requestfile_path("$(OPTICS)", "opticsApp/Db") -set_requestfile_path("$(QUADEM)", "quadEMApp/Db") -set_requestfile_path("$(SSCAN)", "sscanApp/Db") -set_requestfile_path("$(STD)", "stdApp/Db") -set_requestfile_path("$(VAC)", "vacApp/Db") -set_requestfile_path("$(VME)", "vmeApp/Db") -set_requestfile_path("$(TOP)", "xxxApp/Db") - -# Debug-output level -save_restoreSet_Debug(0) - -# END save_restore.cmd -------------------------------------------------------- diff --git a/iocBoot/iocmotorSim/st.cmd.Vx b/iocBoot/iocmotorSim/st.cmd.Vx deleted file mode 100644 index 99b539a85..000000000 --- a/iocBoot/iocmotorSim/st.cmd.Vx +++ /dev/null @@ -1,27 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. - -# The following must be added for many board support packages -#!cd "... IOC st.cmd complete directory path ... " - -< cdCommands -#!< ../nfsCommands - -cd topbin - -# If the VxWorks kernel was built using the project facility, the following must -# be added before any C++ code is loaded (see SPR #28980). -sysCplusEnable=1 - -ld(0,0,"WithAsynVx.munch") - -cd startup -dbLoadDatabase("$(TOP)/dbd/WithAsynVx.dbd") -WithAsynVx_registerRecordDeviceDriver(pdbbase) -dbLoadTemplate("motor.substitutions") - -# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) -motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) -# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) -drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) -iocInit diff --git a/iocBoot/iocmotorSim/st.cmd.allstop b/iocBoot/iocmotorSim/st.cmd.allstop deleted file mode 100644 index a098451ea..000000000 --- a/iocBoot/iocmotorSim/st.cmd.allstop +++ /dev/null @@ -1,56 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. -# -# run with ../../bin/${EPICS_HOST_ARCH}/motorSim.exe - -< envPaths - -# save_restore.cmd needs the full path to the startup directory, which -# envPaths currently does not provide -epicsEnvSet(STARTUP,$(TOP)/iocBoot/$(IOC)) -epicsEnvSet "STOP_ALL_DB" "../../db/stop_all.db" - - -#dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") -#WithAsyn_registerRecordDeviceDriver(pdbbase) -dbLoadDatabase("$(TOP)/dbd/motorSim.dbd") -motorSim_registerRecordDeviceDriver(pdbbase) -dbLoadTemplate("motor.substitutions.local") -#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m1,AS=$(MYPVPREFIX)MTR0:Count") -#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m2,AS=$(MYPVPREFIX)MTR0:Count") -#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m3,AS=$(MYPVPREFIX)MTR0:Count") -#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m4,AS=$(MYPVPREFIX)MTR0:Count") -#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m5,AS=$(MYPVPREFIX)MTR0:Count") -#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m6,AS=$(MYPVPREFIX)MTR0:Count") -#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m7,AS=$(MYPVPREFIX)MTR0:Count") -#dbLoadRecords("$(STOP_ALL_DB)","P=$(MYPVPREFIX),M=m8,AS=$(MYPVPREFIX)MTR0:Count") -dbLoadTemplate("motor.substitutions.allstop") - -### save_restore setup -# We presume a suitable initHook routine was compiled into xxx.munch. -# See also create_monitor_set(), after iocInit() . -#< save_restore.cmd - -# 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 - -# save positions every five seconds -#create_monitor_set("auto_positions.req",5,"P=$(MYPVPREFIX)") -# save other things every thirty seconds -#create_monitor_set("auto_settings.req",30,"P=$(MYPVPREFIX)") - -### Start the saveData task. -#saveData_Init("saveData.req", "P=$(MYPVPREFIX)") diff --git a/iocBoot/iocmotorSim/st.cmd.motorsim b/iocBoot/iocmotorSim/st.cmd.motorsim deleted file mode 100644 index 517f1e6e8..000000000 --- a/iocBoot/iocmotorSim/st.cmd.motorsim +++ /dev/null @@ -1,49 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. -# -# run with ../../bin/${EPICS_HOST_ARCH}/motorSim.exe - -< envPaths - -# save_restore.cmd needs the full path to the startup directory, which -# envPaths currently does not provide -epicsEnvSet(STARTUP,$(TOP)/iocBoot/$(IOC)) - -dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") -WithAsyn_registerRecordDeviceDriver(pdbbase) - -##dbLoadDatabase("$(TOP)/dbd/motorSim.dbd") -##motorSim_registerRecordDeviceDriver(pdbbase) - -##dbLoadTemplate("motor.substitutions.local") -dbLoadRecords("$(TOP)/db/motorSimTest.db") - - -### save_restore setup -# We presume a suitable initHook routine was compiled into xxx.munch. -# See also create_monitor_set(), after iocInit() . -#< save_restore.cmd - -# 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 - -# save positions every five seconds -#create_monitor_set("auto_positions.req",5,"P=$(MYPVPREFIX)") -# save other things every thirty seconds -#create_monitor_set("auto_settings.req",30,"P=$(MYPVPREFIX)") - -### Start the saveData task. -#saveData_Init("saveData.req", "P=$(MYPVPREFIX)") diff --git a/iocBoot/iocmotorSim/st.cmd.sim.inst b/iocBoot/iocmotorSim/st.cmd.sim.inst deleted file mode 100644 index cd9d40c03..000000000 --- a/iocBoot/iocmotorSim/st.cmd.sim.inst +++ /dev/null @@ -1,28 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. -# -# run with ../../bin/${EPICS_HOST_ARCH}/motorSim.exe - -< envPaths - -#dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") -#WithAsyn_registerRecordDeviceDriver(pdbbase) -dbLoadDatabase("$(TOP)/dbd/motorSim.dbd") -motorSim_registerRecordDeviceDriver(pdbbase) -dbLoadTemplate("$(SETTINGS)/motor.substitutions.local") - -# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) -motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) -# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) -drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) - -motorSimCreateController("motorSim2", 8) -#asynSetTraceIOMask("motorSim2", 0, 4) -#asynSetTraceMask("motorSim2", 0, 255) - -# motorSimConfigAxis(port, axis, lowLimit, highLimit, home, start) -motorSimConfigAxis("motorSim2", 0, 20000, -20000, 500, 0) -motorSimConfigAxis("motorSim2", 1, 20000, -20000, 1500, 0) -motorSimConfigAxis("motorSim2", 2, 20000, -20000, 2500, 0) -motorSimConfigAxis("motorSim2", 3, 20000, -20000, 3000, 0) -iocInit diff --git a/iocBoot/iocmotorSim/st.cmd.unix b/iocBoot/iocmotorSim/st.cmd.unix deleted file mode 100644 index 7d693df62..000000000 --- a/iocBoot/iocmotorSim/st.cmd.unix +++ /dev/null @@ -1,27 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. -# -# run with ../../bin/${EPICS_HOST_ARCH}/WithASYN.exe - -< envPaths - -dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") -WithAsyn_registerRecordDeviceDriver(pdbbase) - -dbLoadTemplate("motor.substitutions.local") - -# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) -motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) -# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) -drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) - -motorSimCreateController("motorSim2", 8) -#asynSetTraceIOMask("motorSim2", 0, 4) -#asynSetTraceMask("motorSim2", 0, 255) - -# motorSimConfigAxis(port, axis, lowLimit, highLimit, home, start) -motorSimConfigAxis("motorSim2", 0, 20000, -20000, 500, 0) -motorSimConfigAxis("motorSim2", 1, 20000, -20000, 1500, 0) -motorSimConfigAxis("motorSim2", 2, 20000, -20000, 2500, 0) -motorSimConfigAxis("motorSim2", 3, 20000, -20000, 3000, 0) -iocInit diff --git a/iocBoot/iocmotorSim/st.cmd.win32 b/iocBoot/iocmotorSim/st.cmd.win32 deleted file mode 100755 index 7f861f0c9..000000000 --- a/iocBoot/iocmotorSim/st.cmd.win32 +++ /dev/null @@ -1,14 +0,0 @@ -# The is the ASYN example for communication to 4 simulated motors -# "#!" marks lines that can be uncommented. - -< envPaths - -dbLoadDatabase("$(TOP)/dbd/WithMPFWin32.dbd") -WithMPFWin32_registerRecordDeviceDriver(pdbbase) -dbLoadTemplate("motor.substitutions") - -# Create simulated motors: ( start card , start axis , low limit, high limit, home posn, # cards, # axes to setup) -motorSimCreate( 0, 0, -32000, 32000, 0, 1, 4 ) -# Setup the Asyn layer (portname, low-level driver drvet name, card, number of axes on card) -drvAsynMotorConfigure("motorSim1", "motorSim", 0, 4) -iocInit diff --git a/iocBoot/iocmotorSim/start_medm b/iocBoot/iocmotorSim/start_medm deleted file mode 100755 index 9cd0cba70..000000000 --- a/iocBoot/iocmotorSim/start_medm +++ /dev/null @@ -1 +0,0 @@ -medm -x -macro "P=IOC:, M1=m1, M2=m2, M3=m3, M4=m4, M5=m5, M6=m6, M7=m7, M8=m8" motor8x.adl & From cfc2863fd2f9c3c6472dde382f8d157f73998f65 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Tue, 8 Jul 2025 23:55:50 +0100 Subject: [PATCH 227/232] Merge fixes --- modules/Makefile | 4 + motorApp/Db/SM300_extra.db | 106 ------------------------- motorApp/Db/asyn_motor.db | 2 +- motorApp/Db/asyn_motor_model2.db | 2 +- motorApp/Db/basic_asyn_motor.db | 2 + motorApp/Db/basic_asyn_motor_model2.db | 1 + motorApp/Db/basic_motor.db | 1 + motorApp/Db/motor.db | 2 +- motorApp/MotorSrc/motorRecord.dbd | 2 +- motorApp/MotorSrc/motordevCom.cc | 2 +- 10 files changed, 13 insertions(+), 111 deletions(-) delete mode 100644 motorApp/Db/SM300_extra.db diff --git a/modules/Makefile b/modules/Makefile index 30e86c09b..7093f1b8c 100644 --- a/modules/Makefile +++ b/modules/Makefile @@ -42,6 +42,10 @@ endif SUBMODULES += motorSmartMotor SUBMODULES += motorThorLabs +## isis facility extras +SUBMODULES += motorSM300 +SUBMODULES += motorLinMot + # Allow sites to add extra submodules -include Makefile.local diff --git a/motorApp/Db/SM300_extra.db b/motorApp/Db/SM300_extra.db deleted file mode 100644 index 3001424ff..000000000 --- a/motorApp/Db/SM300_extra.db +++ /dev/null @@ -1,106 +0,0 @@ -# Database for SM300 - -record (busy, "$(P)RESET_AND_HOME") -{ - field(DTYP, "asynInt32") - field(DESC,"Reset and home the motor") - - field(ZNAM, "Done") - field(ONAM, "Reseting") - field(VAL, "0") - field(OUT, "@asyn($(PORT))RESET_AND_HOME") -} - - -record(busy,"$(P)RESET") { - field(DTYP, "asynInt32") - field(DESC,"Reset the motor parameters") - field(ZNAM, "Done") - field(ONAM, "Reseting") - field(VAL, "0") - field(OUT, "@asyn($(PORT))RESET") -} - -record(busy, "$(P)DISCONNECT") { - field(DTYP, "asynInt32") - field(DESC,"Disconnect controller from IOC") - - field(ZNAM, "Done") - field(ONAM, "Disconnecting") - field(VAL, "0") - field(OUT, "@asyn($(PORT))DISCONNECT") -} - -record(mbbi, "$(P)ERROR") { - - field(DTYP, "asynInt32") - field(DESC, "Error code") - field(SCAN, "1 second") - - field(ZRST, "") - field(ZRSV, "NO_ALARM") - field(ZRVL, "0") - - field(ONST, "Servo error") - field(ONSV, "MAJOR") - field(ONVL, "1") - - field(TWST, "Standstill check") - field(TWSV, "MAJOR") - field(TWVL, "2") - - field(THST, "Drag error") - field(THSV, "MAJOR") - field(THVL, "3") - - field(FRST, "Software limit switch") - field(FRSV, "MAJOR") - field(FRVL, "4") - - field(FVST, "Hardware limit switch") - field(FVSV, "MAJOR") - field(FVVL, "5") - - field(SXST, "DAC overflow") - field(SXSV, "MAJOR") - field(SXVL, "6") - - field(SVST, "Unknown") - field(SVSV, "MAJOR") - field(SVVL, "7") - - field(EIST, "Unknown") - field(EISV, "MAJOR") - field(EIVL, "8") - - field(NIST, "Unknown") - field(NISV, "MAJOR") - field(NIVL, "9") - - field(TEST, "No RAM memory card exists") - field(TESV, "MAJOR") - field(TEVL, "10") - - field(ELST, "RAM card not formatted") - field(ELSV, "MAJOR") - field(ELVL, "11") - - field(TVST, "Unknown") - field(TVSV, "MAJOR") - field(TVVL, "12") - - field(TTST, "Cmd rep x4 no success") - field(TTSV, "MAJOR") - field(TTVL, "13") - - field(FTST, "CNC cmd error code") - field(FTSV, "MAJOR") - field(FTVL, "14") - - field(FFST, "Cmd error code") - field(FFSV, "MAJOR") - field(FFVL, "15") - - field(VAL, "0") - field(INP, "@asyn($(PORT))ERROR_CODE") -} diff --git a/motorApp/Db/asyn_motor.db b/motorApp/Db/asyn_motor.db index 9991b78a8..82ebcd424 100644 --- a/motorApp/Db/asyn_motor.db +++ b/motorApp/Db/asyn_motor.db @@ -22,7 +22,7 @@ record(motor, "$(P)$(M)") { field(DLLM, "$(DLLM)") field(INIT, "$(INIT)") field(RTRY, "$(RTRY=10)") - field(RSTM, "$(RSTM=NearZero)") + field(RSTM, "$(RSTM=Never)") field(TWV, "1") field(SDIS, "$(P)$(M)_able.VAL") # ISIS local archiving and alarm diff --git a/motorApp/Db/asyn_motor_model2.db b/motorApp/Db/asyn_motor_model2.db index f7e9254f1..4a2deb05d 100644 --- a/motorApp/Db/asyn_motor_model2.db +++ b/motorApp/Db/asyn_motor_model2.db @@ -22,7 +22,7 @@ record(motor, "$(P)$(M)") { field(DLLM, "$(DLLM)") field(INIT, "$(INIT)") field(RTRY, "$(RTRY=10)") - field(RSTM, "$(RSTM=NearZero)") + field(RSTM, "$(RSTM=Never)") field(TWV, "1") field(SDIS, "$(P)$(M)_able.VAL") } diff --git a/motorApp/Db/basic_asyn_motor.db b/motorApp/Db/basic_asyn_motor.db index b29fbd335..8434f6a44 100644 --- a/motorApp/Db/basic_asyn_motor.db +++ b/motorApp/Db/basic_asyn_motor.db @@ -18,6 +18,7 @@ record(motor,"$(P)$(M)") field(DLLM,"$(DLLM)") field(INIT,"$(INIT)") field(RTRY,"$(RTRY=10)") + field(RSTM,"$(RSTM=Never)") field(TWV,"1") # ISIS local archiving and alarm info(archive, "0.02 VAL RBV DVAL OFF MSTA DIR CNEN MOVN DMOV MISS RCNT") @@ -60,6 +61,7 @@ record(ao,"$(P)$(M)Resolution") { field(DTYP, "asynFloat64") field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_REC_RESOLUTION") field(PREC, "$(PREC)") +} # ISIS enable/disable record(bo, "$(P)$(M)_able") { diff --git a/motorApp/Db/basic_asyn_motor_model2.db b/motorApp/Db/basic_asyn_motor_model2.db index 84e7c1fea..58fd808b1 100644 --- a/motorApp/Db/basic_asyn_motor_model2.db +++ b/motorApp/Db/basic_asyn_motor_model2.db @@ -18,4 +18,5 @@ record(motor,"$(P)$(M)") field(DLLM,"$(DLLM)") field(INIT,"$(INIT)") field(TWV,"1") + field(RSTM,"$(RSTM=Never)") } diff --git a/motorApp/Db/basic_motor.db b/motorApp/Db/basic_motor.db index 239e673f1..e542840e5 100644 --- a/motorApp/Db/basic_motor.db +++ b/motorApp/Db/basic_motor.db @@ -17,6 +17,7 @@ grecord(motor,"$(P)$(M)") field(DHLM,"$(DHLM)") field(DLLM,"$(DLLM)") field(INIT,"$(INIT)") + field(RSTM,"$(RSTM=Never)") field(TWV,"1") # ISIS local archiving and alarm info(archive, "0.02 VAL RBV DVAL OFF MSTA DIR CNEN MOVN DMOV MISS RCNT") diff --git a/motorApp/Db/motor.db b/motorApp/Db/motor.db index 6759975af..76ed0740a 100644 --- a/motorApp/Db/motor.db +++ b/motorApp/Db/motor.db @@ -23,7 +23,7 @@ record(motor, "$(P)$(M)") { field(DHLM, "$(DHLM)") field(DLLM, "$(DLLM)") field(INIT, "$(INIT)") - field(RSTM, "$(RSTM=NearZero)") + field(RSTM, "$(RSTM=Never)") field(TWV, "1") field(SDIS, "$(P)$(M)_able.VAL") # ISIS local archiving and alarm diff --git a/motorApp/MotorSrc/motorRecord.dbd b/motorApp/MotorSrc/motorRecord.dbd index 806a10997..f7e7c1360 100644 --- a/motorApp/MotorSrc/motorRecord.dbd +++ b/motorApp/MotorSrc/motorRecord.dbd @@ -848,7 +848,7 @@ recordtype(motor) { interest(2) } field(RSTM,DBF_MENU) { - initial("NearZero") + initial("Never") prompt("Restore Mode") promptgroup(GUI_COMMON) interest(2) diff --git a/motorApp/MotorSrc/motordevCom.cc b/motorApp/MotorSrc/motordevCom.cc index 85ae97282..a66e7b762 100644 --- a/motorApp/MotorSrc/motordevCom.cc +++ b/motorApp/MotorSrc/motordevCom.cc @@ -194,7 +194,7 @@ motor_init_record_com(struct motorRecord *mr, int brdcnt, struct driver_table *t double ep_mp[2]; /* encoder pulses, motor pulses */ int rtnStat; msta_field msta; - bool use_rel = false; //(mr->rtry != 0 && mr->rmod != motorRMOD_I && (mr->ueip || mr->urip)); + bool use_rel = (mr->rtry != 0 && mr->rmod != motorRMOD_I && (mr->ueip || mr->urip)); /* allocate space for private field - an motor_trans structure */ mr->dpvt = (struct motor_trans *) malloc(sizeof(struct motor_trans)); From b461f9816ff6ac6a45d10c48e86a1c2b300615bb Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Wed, 9 Jul 2025 00:31:18 +0100 Subject: [PATCH 228/232] Update submodules --- .gitmodules | 2 ++ modules/motorLinMot | 2 +- modules/motorMclennan | 2 +- modules/motorMotorSim | 2 +- modules/motorNewport | 2 +- modules/motorSM300 | 2 +- 6 files changed, 7 insertions(+), 5 deletions(-) diff --git a/.gitmodules b/.gitmodules index 2a0775ec3..a1dbc6386 100644 --- a/.gitmodules +++ b/.gitmodules @@ -103,6 +103,8 @@ [submodule "modules/motorSM300"] path = modules/motorSM300 url = https://github.com/ISISComputingGroup/motorSM300.git + branch = main [submodule "modules/motorLinMot"] path = modules/motorLinMot url = https://github.com/ISISComputingGroup/motorLinMot.git + branch = main diff --git a/modules/motorLinMot b/modules/motorLinMot index ca9417758..ef7c629df 160000 --- a/modules/motorLinMot +++ b/modules/motorLinMot @@ -1 +1 @@ -Subproject commit ca941775851a7e9265c5cfa58f1d86d58916d0f8 +Subproject commit ef7c629df4c56e6e9d502020508a140d5cdd8f43 diff --git a/modules/motorMclennan b/modules/motorMclennan index 2024aa22d..52b4198a2 160000 --- a/modules/motorMclennan +++ b/modules/motorMclennan @@ -1 +1 @@ -Subproject commit 2024aa22dc0a9b3ea8c53341ac74d94698aa8add +Subproject commit 52b4198a26874ab2cff3a7ac2315c88e6531ad87 diff --git a/modules/motorMotorSim b/modules/motorMotorSim index 9382836c7..8e1bfe7f3 160000 --- a/modules/motorMotorSim +++ b/modules/motorMotorSim @@ -1 +1 @@ -Subproject commit 9382836c729c705de51ecfd6af57e2aa6e558995 +Subproject commit 8e1bfe7f3f911edaedab5018e239c777ffd6f14a diff --git a/modules/motorNewport b/modules/motorNewport index 395031053..06511fdff 160000 --- a/modules/motorNewport +++ b/modules/motorNewport @@ -1 +1 @@ -Subproject commit 395031053f141d9980b3de489945ce7da47469cc +Subproject commit 06511fdff43db058e53874430af161728515ff97 diff --git a/modules/motorSM300 b/modules/motorSM300 index 14e61a563..aae63fbbb 160000 --- a/modules/motorSM300 +++ b/modules/motorSM300 @@ -1 +1 @@ -Subproject commit 14e61a56308390127711df94fbf14fc4cefa749f +Subproject commit aae63fbbb58ddfeb2d2fd69584a1f5af01bb4d67 From e4e3f86cd12af1b9c6f645286d989af83115bc92 Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 11 Jul 2025 00:12:53 +0100 Subject: [PATCH 229/232] Update submodules --- .gitmodules | 8 +++----- modules/motorMotorSim | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/.gitmodules b/.gitmodules index a1dbc6386..99b7c2878 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,7 @@ [submodule "modules/motorNewport"] path = modules/motorNewport url = https://github.com/ISISComputingGroup/motorNewport.git - branch = ibex + branch = ibex [submodule "modules/motorOms"] path = modules/motorOms url = https://github.com/epics-motor/motorOms.git @@ -26,7 +26,7 @@ [submodule "modules/motorMclennan"] path = modules/motorMclennan url = https://github.com/ISISComputingGroup/motorMclennan.git - branch = ibex + branch = ibex [submodule "modules/motorMicroMo"] path = modules/motorMicroMo url = https://github.com/epics-motor/motorMicroMo.git @@ -87,7 +87,7 @@ [submodule "modules/motorMotorSim"] path = modules/motorMotorSim url = https://github.com/ISISComputingGroup/motorMotorSim.git - branch = ibex + branch = ibex [submodule "modules/motorMXmotor"] path = modules/motorMXmotor url = https://github.com/epics-motor/motorMXmotor.git @@ -103,8 +103,6 @@ [submodule "modules/motorSM300"] path = modules/motorSM300 url = https://github.com/ISISComputingGroup/motorSM300.git - branch = main [submodule "modules/motorLinMot"] path = modules/motorLinMot url = https://github.com/ISISComputingGroup/motorLinMot.git - branch = main diff --git a/modules/motorMotorSim b/modules/motorMotorSim index 8e1bfe7f3..229fb86c3 160000 --- a/modules/motorMotorSim +++ b/modules/motorMotorSim @@ -1 +1 @@ -Subproject commit 8e1bfe7f3f911edaedab5018e239c777ffd6f14a +Subproject commit 229fb86c34318cd6d01d40fec7443f047c0695a8 From 872d174e1f07c722de7e574600c02ceebe7fe50d Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 11 Jul 2025 00:20:22 +0100 Subject: [PATCH 230/232] Update submodules --- modules/motorLinMot | 2 +- modules/motorMclennan | 2 +- modules/motorSM300 | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/motorLinMot b/modules/motorLinMot index ef7c629df..48ee8f5d1 160000 --- a/modules/motorLinMot +++ b/modules/motorLinMot @@ -1 +1 @@ -Subproject commit ef7c629df4c56e6e9d502020508a140d5cdd8f43 +Subproject commit 48ee8f5d17a7aaa7a83cf59480e7b43530d190f0 diff --git a/modules/motorMclennan b/modules/motorMclennan index 52b4198a2..2ad6227ad 160000 --- a/modules/motorMclennan +++ b/modules/motorMclennan @@ -1 +1 @@ -Subproject commit 52b4198a26874ab2cff3a7ac2315c88e6531ad87 +Subproject commit 2ad6227ad6efb2b36749e8f7e6e2b0cba0b35b08 diff --git a/modules/motorSM300 b/modules/motorSM300 index aae63fbbb..7b5adf544 160000 --- a/modules/motorSM300 +++ b/modules/motorSM300 @@ -1 +1 @@ -Subproject commit aae63fbbb58ddfeb2d2fd69584a1f5af01bb4d67 +Subproject commit 7b5adf544474d514267463afc7409d6d774f6cbb From 5afdedd7562503ffe5bb17b6c26a7e3f5c6c2b9b Mon Sep 17 00:00:00 2001 From: Freddie Akeroyd Date: Fri, 11 Jul 2025 00:33:24 +0100 Subject: [PATCH 231/232] Update submodules --- modules/motorLinMot | 2 +- modules/motorSM300 | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/motorLinMot b/modules/motorLinMot index 48ee8f5d1..85308e394 160000 --- a/modules/motorLinMot +++ b/modules/motorLinMot @@ -1 +1 @@ -Subproject commit 48ee8f5d17a7aaa7a83cf59480e7b43530d190f0 +Subproject commit 85308e394d0e9b57a963290cad578c5df74f8612 diff --git a/modules/motorSM300 b/modules/motorSM300 index 7b5adf544..92baa9153 160000 --- a/modules/motorSM300 +++ b/modules/motorSM300 @@ -1 +1 @@ -Subproject commit 7b5adf544474d514267463afc7409d6d774f6cbb +Subproject commit 92baa91539c95f01295aac5ee73f9c03a96e5c41 From 79e719d7834adb44c8480397b709892cecf737ca Mon Sep 17 00:00:00 2001 From: Jack Harper Date: Mon, 28 Jul 2025 15:14:45 +0100 Subject: [PATCH 232/232] Remove SM300 motor module as no longer used --- .gitmodules | 3 --- modules/Makefile | 1 - 2 files changed, 4 deletions(-) diff --git a/.gitmodules b/.gitmodules index 99b7c2878..26d7a3326 100644 --- a/.gitmodules +++ b/.gitmodules @@ -100,9 +100,6 @@ [submodule ".ci"] path = .ci url = https://github.com/epics-base/ci-scripts -[submodule "modules/motorSM300"] - path = modules/motorSM300 - url = https://github.com/ISISComputingGroup/motorSM300.git [submodule "modules/motorLinMot"] path = modules/motorLinMot url = https://github.com/ISISComputingGroup/motorLinMot.git diff --git a/modules/Makefile b/modules/Makefile index 7093f1b8c..d12467910 100644 --- a/modules/Makefile +++ b/modules/Makefile @@ -43,7 +43,6 @@ SUBMODULES += motorSmartMotor SUBMODULES += motorThorLabs ## isis facility extras -SUBMODULES += motorSM300 SUBMODULES += motorLinMot # Allow sites to add extra submodules