#%TITLE% ICE_TRAJ.MAC
#
#%NAME%
# Macros to handle IcePAP trajectories.
#
#%DESCRIPTION%
# These macros allow to create and download an IcePAP trajectory.
#
# The aim is to move a set of IcePAP motors synchronously and
# following a pre-loaded trajectory. The trajectory is a list of
# positions for a given parameter value.
#
# To move over the trajectory, a macro motor has to be configured
# and it's position value will be the parameter value. Note that
# the parameter value can be inbetween parameter values loaded
# (the IcePAP DRIVER does interpolation).
#
# The macro motor velocity/acctime of the config are used during
# trajectory motion.
#
#%DL%
#%DT%Configuring macro motor:
# %DL%
# %DT% 1)
# You must have an entry in "MOTORS" table:
# %DL%
# %DT% -
# The "DEVICE" field must be set to string "icepap_traj"
# %DT% -
# The "ADDR" must be set to the MASTER network name.
# (ex: "iceid033")
# %DT% -
# The "NUM" should be set to at least 1
# %DT% -
# The "TYPE" field must be set to "Macro Motors"
# %XDL%
# %DT% 2)
# Declare on macro motor with:
# %DL%
# %DT% -
# The "Controller" field set to "MAC_MOT"
# %DT% -
# The "Unit" field, numbered from 0,
# must be set to the MOTORS entry.
# %DT% -
# The "Module"/"Chan" fields don't mind.
# %DT% -
# Add a Custom Parameter "axis_list" with the list of
# IcePAP motors mnemonics that will be part of the trajectory
# (ex: "del gam mu")
# %XDL%
# %XDL%
#
#%DT%Extra motor_par() parameters:
# %DL%
# %DT%motor_par(motor, "sync")%DD%
# %DT%motor_par(motor, "sync", par_val)%DD%
# Try to put the concerned motors on the trajectory
# at the given parameter value if given. WARNING: the motors will move.
# Always returns if all the motors are on the trajectory ("1") or not ("0")
#
# %DT%motor_par(motor, "power")%DD%
# %DT%motor_par(motor, "power", 1)%DD%
# Try to enable the power on all the motors concerned by the trajectory.
# Always returns if all the motors are powered ("1") or not ("0")
#
# %DT%motor_par(motor, "traj_infos")%DD%
# Returns a string with "interpolation par_min par_max npoints"
# corresponding to the current loaded trajectory. Note: if
# trajectory is defined for several motors, the info returns is
# the first one motor only.
#
# %DT%motor_par(motor, "slew_rate_max")%DD%
# Returns the maximum parameter velocity for the trajectory
# currenltly loaded.
# %XDL%
#%XDL%
#
#%EXAMPLE%
#%DL%
#%DT%Macro motor usage:
#%PRE%
# icepap_traj_load mytraj parmot
# icepap_traj_sync parmot 0.0
# mv parmot 3.5
#
#%DT%Low level commands:
#%PRE%
# short array mypar[4]
# short array mypos[4]
# array_op("fill", mypar)
# array_op("fill", mypos, 10)
# icepap_traj_build mytraj PARAMETER mypar 6
# icepap_traj_build mytraj POSITION mypos 6
# icepap_traj_info mytraj
# icepap_traj_load mytraj iceid321 6
# icepap_traj_delete mytraj
#
# icepap_traj_build mytraj2 PARAMETER mypar 255
# icepap_traj_build mytraj2 POSITION mypos 6
# icepap_traj_load mytraj2 iceid321
#
# icepap_traj_sync parmot 0.0
# mv parmot 3.5
#
#%XDL%
#%END%
#
# Mandatory IcePAP data vector macros
need ice_vdata
#%UU% trajectory type array [addr]
#%MDESC%
# Update a trajectory with the content of the given
# data array. If the trajectory does not exist, it will be created,
# otherwise it will be enlarged with the new data.
#
# The type describe the kind of vector data which could be
# "PARAMETER", "POSITION", "SLOPE"
#
# The address given is the destination DRIVER address.
# When giving the parameter, the address is not mandatory.
# Then the parameter will be used for all afterward DRIVERs.
#
# Example:
# icepap_traj_build mytraj1 PARAMETER mypar
# icepap_traj_build mytraj1 POSITION mypos 6
#
def icepap_traj_build '{
local addr
if($# < 3) {
p "Usage: $0 traj type array [addr]"
p " Ex: $0 mytraj1 POSITION posarray 7"
exit
}
addr = $4
if(addr == 0) {
if(icepap__toupper("$2") == "PARAMETER") {
addr = 255
} else {
p "Missing destination DRIVER address"
exit
}
}
if(_icepap_traj_build("$1", "$2", $3, addr)) {
exit
}
}'
#%IU%(traj_name, type, array, addr, silent)
#%MDESC%
# Update an IcePAP trajectory, given by its name.
#
# Returns non null if an error occured.
#
def _icepap_traj_build(traj_name, trajt_str, data, addr, silent) '{
global ICEPAP_TRAJ_VERB
local verb
verb = (silent)?0:ICEPAP_TRAJ_VERB
if(verb & ICEPAP_TRAJ_VTRAJ) {
printf("\tTrajectory : %s\n", traj_name)
}
return(_icepap_vdata_build(traj_name, trajt_str, data, addr, \
!(verb&ICEPAP_TRAJ_VTRAJ)))
}'
#%UU% trajectory
#%MDESC%
# Delete a trajectory.
#
# Example:
# icepap_traj_delete mytraj1
#
def icepap_traj_delete '{
if($# != 1) {
p "Usage: $0 traj"
p " Ex: $0 mytraj1"
exit
}
if(_icepap_traj_delete("$1")) {
exit
}
}'
#%IU%(traj_name)
#%MDESC%
# Delete a trajectory.
#
def _icepap_traj_delete(traj_name) '{
# remove any previous information
_icepap_vdata_cleanup(traj_name)
# remove from memory
#unglobal @traj_name
short array @traj_name[1]
return(0)
}'
#%UU% trajectory
#%MDESC%
# Example:
# icepap_traj_info mytraj1
#
def icepap_traj_info '{
if($# != 1) {
p "Usage: $0 traj"
p " Ex: $0 mytraj1"
exit
}
icepap_vdata_info $1
}'
#%UU% trajectory { param_motor } | { hostname [address] }
#%MDESC%
# Download the given trajectroy into the specified IcePAP system.
#
# If an address is given, the trajectory will be sent
# directly to the concerned DRIVER.
#
# Example:
# icepap_traj_load mytraj1 iceid321 6
# icepap_traj_load mytraj1 m0
#
def icepap_traj_load '{
local addr
if($# < 2) {
p "Usage: $0 trajectory { param_motor } | { hostname [address] }"
p " Ex: $0 mytraj1 iceid321 6"
p " Ex: $0 mytraj1 m0"
exit
}
addr = $3
if(_icepap_traj_load("$1", "$2", addr)) {
exit
}
}'
#%IU%(traj_name, hostname, addr, silent)
#%MDESC%
# Download a trajectroy, given by its name, into the specified IcePAP system.
#
# If the address is not 0, then the trajectory will be sent
# directly to a DRIVER.
#
# Returns non null if an error occured.
#
# TODO: -check that the trajectory contains only good addresses
#
# Example:
# _icepap_traj_load("mytraj1", "iceid321", 6)
# _icepap_traj_load("mytraj1", "iceid321")
# _icepap_traj_load("mytraj1", "m0")
#
def _icepap_traj_load(traj_name, argin, addr, silent) '{
global ICEPAP_TRAJ_VERB
local verb
local addr addr_s
local ans i
local to_ms bin_sz
local tbeg tend
local mne num hostname
local p1
# guess if a motor or an IcePAP hostname has been givent
if((num = motor_num(argin)) != -1) {
# minimum checks here on motor type
mne = argin
if(motor_par(num, "device_id") != "icepap_traj") {
_icepap_err
p "invalid motor \"" mne "\" not an IcePAP trajectory motor"
return(-1)
}
hostname = motor_par(num, "address")
silent = addr
} else {
mne = ""
hostname = argin
}
#
verb = (silent)?0:ICEPAP_TRAJ_VERB
# Warning: when transfering large amount of data
# the timeouts have to be adjusted
bin_sz = array_op("cols", @traj_name) * array_op("rows", @traj_name)
to_ms = (bin_sz * 4.0) / 100.0
if(to_ms < ICEPAP_TIMEOUT*1000) {
to_ms = ICEPAP_TIMEOUT*1000
}
if(verb & ICEPAP_TRAJ_VTRAJ) {
printf("\tTrajectory : %s\n", traj_name)
printf("\tDestination : %s\n", hostname)
printf("\tNew timeout : %dmsec\n", to_ms)
if(addr) {
printf("\tSent to driver: %d\n", addr)
}
}
# change timeouts
_icepap_change_timeout(hostname, to_ms)
# send the binary
# TODO: remove address argin / convert to optional argin
addr_s = (addr)?sprintf("#%d", addr) : "#"
tbeg = time()
ans = _icepap_send_array(hostname, addr_s, "*PARDAT", @traj_name,\
!(verb&ICEPAP_TRAJ_VDUMP))
tend = time()
if(verb & ICEPAP_TRAJ_VTRAJ) {
printf("\tLoad duration : %dmsec\n", (tend-tbeg)*1000.0)
}
# restore timeouts
_icepap_change_timeout(hostname, ICEPAP_TIMEOUT*1000)
# check for errors
if(i = index(ans,"ERROR")) {
_icepap_err
print substr(ans, i+6)
return(-1)
}
if(ans == "") {
_icepap_err
print "timeout waiting for acknowledge from:", hostname
return(-1)
}
# mandatory initialization of parameter velocity/acctime after
# each new trajectory load
if(mne != "") {
local vel_max
# set slew_rate (p1=rate in Hz)
p1 = motor_par(num, "slew_rate")
# TODO: check for each real axis the param velocity boundaries
# and check that they are compatible with parameter motor velocity
vel_max = motor_par(num, "max_velocity")
if(p1 > vel_max) {
_icepap_err
printf("invalid velocity (%.1f>%.1f) for motor: \"%s\"\n", \
p1, vel_max, mne)
return(-1)
}
# BUG of firmware 3.17:
# the parameter velocity must be set to 0 first,
# but only if the trajectory flat
if(0) {
# NOTO: fixed in firmware 3.187
icepap_traj_cmd(num, "slew_rate", 0)
}
icepap_traj_cmd(num, "slew_rate", p1)
# set acceleration (p1=ms p2=steps/sec^2)
p1 = motor_par(num, "acceleration")
icepap_traj_cmd(num, "acceleration", p1)
# keep a record a trajectory array
motor_par(num, "traj_name", traj_name, "add")
}
# normal end
return(0)
}'
#%IU%(hostname, timeout_ms)
#%MDESC%
# Change communication timeout
#
def _icepap_change_timeout(hn, to_ms) '{
local dev
dev = sprintf("%s%s", hn, index(hn,":")?"":":5000")
sock_par(dev, "timeout", to_ms/1000.0)
_icepap_wr(dev, "", sprintf("_FIFOTO %d", to_ms))
}'
#%UU% motor parameter_value
#%MDESC%
# Put all concerned real motors on the trajectory at the positions
# corresponding to the given parameter value.
#
def icepap_traj_sync '{
local num
local mne
local pos
if($# != 2) {
p "Usage: $0 motor parameter_value"
p " Ex: $0 param1 0.0"
exit
}
mne = "$1"
pos = $2
num = motor_num(mne)
if(num == -1) {
_icepap_err
p "invalid motor \"" mne "\""
exit
}
if(motor_par(num, "device_id") != "icepap_traj") {
_icepap_err
p "invalid motor \"" mne "\" not an IcePAP trajectory motor"
exit
}
p "WARNING: all concerned physical motors will move"
if(!yesno("Continue?", 0)) {
exit
}
_icepap_traj_sync(num, pos)
}'
#%UU% motor parameter_value
#%MDESC%
# Put all concerned real motors on the trajectory at the positions
# corresponding to the given parameter value.
# Returns non null if an error occured.
#
def _icepap_traj_sync(num, pos, silent) '{
# launch motions
motor_par(num, "sync", pos)
# retrieve information on read IcePAP axes
local i n mnes[] nums[]
local stat stats w
n = split(motor_par(num, "axis_list"), mnes)
if(!n) {
_icepap_err
p "wrong config for motor \"" mne "\""
return(-1)
}
# wait for the end of the motions
w = 10
for(i=0;i<n;i++) {
if(!silent) { printf("%*s ", w, mnes[i]) }
nums[i] = motor_num(mnes[i])
}
if(!silent) { printf("\n") }
for(inloop=1;inloop; sleep(0.5)) {
for(i=0, inloop=0;i<n;i++) {
stat = icepap_cmd(nums[i], "get_status")
if(!silent) {
printf("%*s ", w, stat?"MOVING":"READY")
}
inloop += stat
}
if(!silent) { printf("\r") }
}
if(!silent) { printf("\n") }
# update SPEC positions after the motions that it is not aware
read_motors(0x06)
# normal end
return(0)
}'
#%IU%(motor, silent)
#%MDESC%
# Check if the given parameter motor is ready to be moved.
# If silent is non null, no message will printed out.
# Returns 0 if not ready.
#
def _icepap_traj_check_ready(num, silent) '{
local ret
local dev
local mne
local ipol pmin pmax np
local addr_list addrs[] addr n
# not valid case
ret = 0
# check motor type
mne = motor_mne(num)
if(motor_par(num, "device_id") != "icepap_traj") {
if(!silent) {
_icepap_err
p "invalid motor \"" mne "\" not an IcePAP trajectory motor"
}
return(ret)
}
# get the IcePAP system
dev = motor_par(num, "address")
# get the list of real motors to control
if((addr_list = motor_par(num, "addr_list")) == 0) {
if(!silent) {
_icepap_err
printf("missing \"addr_list\" parameter for motor: \"%s\"\n", mne)
}
return(ret)
}
split(addr_list, addrs)
# check if parametric trajectory is already downloaded and
# then get parameter boundaries
ipol = "UNKNOWN"
pmin = -1
pmax = -1
np = -1
for(n in addrs) {
addr = addrs[n]
# answer should be "SPLINE param_beg param_end n_param"
ans = _icepap_query(dev, addr, "?PARDAT", silent)
if(index(ans, "ERROR Uninitialised parametric table")) {
if(!silent) {
_icepap_warn
printf("Uninitialised parametric table for axis \"%s\"\n", \
sprintf("%s:%d", dev, addr))
_icepap_warn
printf("Hint: use \"icepap_traj_load\"\n")
}
return(ret)
}
# get parameter boundaries
if(sscanf(ans, "%s %f %f %d", ipol, pmin, pmax, np) != 4) {
return(ret)
}
# has not to be the same for each axis but as no physical sense
# therefore consider that all axes should have the same
}
# check that each concerned real motor is powered
if(motor_par(num, "power") != 1) {
if(!silent) {
_icepap_err
printf("missing power on one of the motors used by trajectory\n")
}
return(ret)
}
# TODO: change macro motor software limits or not??
if(0) {
if((pmin != -1) && (pmax != -1)) {
set_lim(num, pmin, pmax)
}
}
# check that we are on the trajectory
if(!motor_par(num, "sync")) {
if(!silent) {
_icepap_err
printf("\"%s\" not on trajectory, missing \"icepap_traj_sync\"\n", mne)
}
return(ret)
}
# normal end
return(ret?0:1)
}'
#%IU%(array_name, hostname, address, npoints, silent)
#%MDESC%
# Fill in the given array with the trajectory read positions
# read from the given IcePAP DRIVER.
# Note: if the requested number of points is higher that the loaded
# one, then the DRIVER will interpolated.
#
def _icepap_traj_read(pos_arr, dev, addr, npoints, silent) '{
local par par_ipol par_beg par_end npar par_delta
local ans
local m pos
# get information on current loaded parametric table
# answer should be "SPLINE|LINEAR par_beg par_end npar"
ans = _icepap_query(dev, addr, "?PARDAT")
if(index(ans, "ERROR")) {
exit
}
if(sscanf(ans, "%s %f %f %d", par_ipol, par_beg, par_end, npar) != 4) {
_icepap_err
p "unable to get information on loaded parametric table"
exit
}
# parameter delta
par_delta = (par_end - par_beg) / (npoints-1)
float array @pos_arr[npoints]
for(n=0;n<npoints;n++) {
par = par_beg + (par_delta*n)
ans = _icepap_query(dev, addr, sprintf("?PARVAL %f", par))
if(index(ans, "ERROR")) {
exit
}
sscanf(ans, "%f", pos)
@pos_arr[n] = pos
}
}'
#%IU% traj_motor npoints
#%MDESC%
# FOR TESTS:
# Bench the trajectory load time.
#
# The motor given has to be an "icepap_traj" one with its list
# of real IcePAP motors concerned by the trajectory.
#
# WARNING: the macro will generate a trajectory and load it into
# the given IcePAP DRIVERs overwriting any previous trajectory.
#
# Example:
# _icepap_traj_bench m1 100
#
def _icepap_traj_bench '{
local npts num mne
local dev addrs[]
local n i
local tbeg tend
if($#!=2) {
p "Usage: $0 npoints traj_motor"
p " Ex: $0 m1 100"
exit
}
mne = "$1"
npts = $2
num = motor_num(mne)
if(motor_par(num, "device_id") != "icepap_traj") {
_icepap_err
p "invalid motor \"" mne "\" not an IcePAP trajectory motor"
exit
}
n = split(motor_par(num, "addr_list"), addrs)
float array myparam[npts]
float array mypos[npts]
array_op("fill", myparam, (2*PI)/(npts-1.0))
mypos = sin(myparam)*100
silent = 1
_icepap_traj_delete("mytraj")
tbeg=time()
_icepap_traj_build("mytraj", "parameter", myparam, 255, silent)
for(i=0;i<n;i++) {
_icepap_traj_build("mytraj", "position", mypos, addrs[i], silent)
}
tbeg = time()-tbeg
p ""
printf("Details on the trajectory vdata:\n")
icepap_traj_info mytraj
p ""
printf("Trajectory number of points : %d\n", npts)
printf("Trajectory number of motors : %d\n", n)
printf("Trajectory vdata total size : %.3fKb\n", \
0.002*array_op("cols",mytraj))
printf("Generate trajectory vdata in: %.2fms\n", tbeg*1000.0)
tbeg=time()
_icepap_traj_load("mytraj", mne, silent)
tbeg = time()-tbeg
printf("Loading trajectory vdata in: %.2fms\n", tbeg*1000.0)
}'
#%IU% hostname address [traj_points_wr [traj_points_rd]]
#%MDESC%
# FOR TESTS:
# Check interpolation done by the IcePAP DRIVER on a loaded trajectory.
# Displays the absolute error between the interpolated trajectory
# and the theoritical one.
#
# WARNING: the macro will generate a trajectory and load it into
# the given IcePAP DRIVER overwriting any previous trajectory.
#
# The default number of points of the loaded trajectory is 100
# while the default number of points of the read one is 1000.
#
# Example:
# _icepap_traj_test iceid321 6
# _icepap_traj_test iceid321 6 100
# _icepap_traj_test iceid321 6 100 1000
#
def _icepap_traj_test '{
local dev addr
local i n
local silent
local npts_rd npts_wr
if($#<2) {
p "Usage: $0 hostname addr [npts_wr [npts_rd]]"
p " Ex: $0 iceid321 6"
exit
}
dev = "$1"
addr = $2
npts_wr = ($#>=3)?$3:100.0
npts_rd = ($#>=4)?$4:1000.0
float array myparam100[npts_wr]
float array myparam[npts_rd]
float array myerr[npts_rd]
float array mypos100[npts_wr]
float array mypos[npts_rd]
array_op("fill", myparam100, (2*PI)/(npts_wr-1.0))
mypos100 = sin(myparam100)*100
array_op("fill", myparam, (2*PI)/(npts_rd-1.0))
mypos = sin(myparam)*100
silent = 1
_icepap_traj_delete("mytraj100")
_icepap_traj_build("mytraj100", "position", mypos100, addr, silent)
_icepap_traj_build("mytraj100", "parameter", myparam100, addr, silent)
_icepap_traj_load("mytraj100", dev, 0, silent)
_icepap_traj_read("myread", dev, addr, npts_rd, silent)
myerr = mypos - myread
plot_cntl(sprintf("title=Interpolated trajectory error"))
plot_cntl(sprintf("colors=%s",splot_col))
plot_cntl("open")
plot_cntl("erase")
plot_cntl("+dots -ebars +lines -xlog -ylog")
plot_range("auto","auto","auto","auto")
plot_move(10, 2, sprintf("Trajectory points loaded: %d read: %d", \
npts_wr, npts_rd))
plot_move(10, 1, sprintf("Tracjectory loaded: %s interpolation: %s", \
"100*sin(2*PI)", "SPLINE"))
array_plot(myparam, myerr)
}'
constant ICEPAP_TRAJ_VNONE 0x00
constant ICEPAP_TRAJ_VTRAJ 0x01
constant ICEPAP_TRAJ_VDUMP 0x02
constant ICEPAP_TRAJ_VMACMOT 0x04
#%UU% verbose_level
#%MDESC%
# Set the verbose level of trajectory macros, 0==none
#
def icepap_traj_debug '{
global ICEPAP_TRAJ_VERB
if($# == 1) {
if($1 < 0) {
_icepap_err
p "invalid verbose level, must be >0"
exit
}
ICEPAP_TRAJ_VERB = $1
}
printf("Current verbose level: 0x%x\n", ICEPAP_TRAJ_VERB)
printf(" 0x%x no messages\n", ICEPAP_TRAJ_VNONE)
printf(" 0x%x trajectory building\n", ICEPAP_TRAJ_VTRAJ)
printf(" 0x%x dump of trajectory\n", ICEPAP_TRAJ_VDUMP)
printf(" 0x%x macro motor\n", ICEPAP_TRAJ_VMACMOT)
}'
#%IU%(num, type, p1, p2, p3)
#%MDESC%
# MACRO MOTOR:
# Called by spec after reading the config file
#
def icepap_traj_config(num, type, p1, p2, p3) '{
global ICEPAP_TRAJ[]
local silent
silent = 1
# p1==controller index p2==number of motors supported
if(type=="ctrl") {
local dev
# remove any previous configuration
if (p1 == 0) {
for(i in ICEPAP_TRAJ) {
delete ICEPAP_TRAJ[i]
}
}
# get IcePAP system hostname
dev = icepap_traj_ADDR
if(dev == "") {
_icepap_err
printf("missing ADDR field\n")
return ".error."
}
# if the MASTER is switched off, disable all the associated motors
silent = 1
if(_icepap_query(dev, 0, "?ID", silent) == "") {
return ".error."
}
# normal end
return
}
# p1==unit p2==module p3==channel
if(type=="mot") {
local mne
local dev
local ans
local mnes[] n i
local mne_ice mne_list
local addr addr_list
local silent
# get the motor mnemonic string
mne = motor_mne(num)
dev = icepap_traj_ADDR
# get the list of real motors to control
if((mne_list = motor_par(num, "axis_list")) == 0) {
_icepap_err
printf("missing \"axis_list\" parameter for motor: \"%s\"\n", mne)
return ".error."
}
# for each concerned real motor
silent = 1
addr_list = ""
n = split(mne_list, mnes)
for(i=0;i<n;i++) {
mne_ice = mnes[i]
num_ice = motor_num(mne_ice)
# check that each motor is an IcePAP one
if(motor_par(num_ice, "device_id") != "icepap") {
_icepap_err
printf("invalid motor \"%s\" in \"axis_list\" for motor: \"%s\"\n", \
mne_ice, mne)
return ".error."
}
# try to power on each concerned motor
if(motor_par(num_ice, "power", 1) != 1) {
_icepap_err
printf("unable to power the following motor used by trajectory\n")
# keep the motor usable
# return ".error."
}
# at this point we have a valide IcePAP motor
addr = motor_par(num_ice,"module")*10
addr += motor_par(num_ice,"channel")
addr_list = sprintf("%s%d ", addr_list, addr)
}
# can not check if the trajectory is loaded, too early on the reconfig
# normal end
motor_par(num, "addr_list", addr_list, "add")
return
}
}'
#%IU%(dev, addr)
#%MDESC%
# Returns the motor mne corresponding to the hardware address given.
# If not motor is found, the ".error." is returned.
#
def _icepap_addr2mne(hostname, addr) '{
local dev
local mne
local num
dev = index(hostname, ":")?hostname:sprintf("%s:5000",hostname)
for(num=0;num<MOTORS;num++) {
mne = motor_mne(num)
if(ICEPAP[mne]["dev"] != dev) {
continue
}
if(ICEPAP[mne]["addr"] == addr) {
return(mne)
}
}
return ".error."
}'
#%IU%(num, key, todo, p1)
#%MDESC%
# MACRO MOTOR:
# Called by spec on motor parameters access.
#
def icepap_traj_par(num, key, todo, p1) '{
local mne
local dev
local cmd
local addr_list
local stpz
# return new motor_par() argins handled
if (key == "?" && todo == "get") {
return("axis_list, sync, power, max_velocity, traj_infos")
}
# get the motor mnemonic string
mne = motor_mne(num)
dev = motor_par(num, "address")
addr_list = motor_par(num, "addr_list")
stpz = motor_par(num, "step_size")
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
local tt
tt = (todo=="set")?sprintf("%f", p1):""
icepap_traj_print sprintf("\"%s\" \"%s\" \"%s\" %s", mne, key, todo, tt)
}
# power on axes of the paramtric trajectory
if (key == "power") {
local i n mnes[] nums[]
n = split(motor_par(num, "axis_list"), mnes)
# try to switch power on axes of the paramtric trajectory
if(todo == "set") {
for(i=0;i<n;i++) {
if(motor_par(mnes[i], "power", 1) != 1) {
return(0)
}
}
}
# always return the current state
for(i=0;i<n;i++) {
if(motor_par(mnes[i], "power") != 1) {
return(0)
}
}
return(1)
}
# put axes on the paramtric trajectory
if (key == "sync") {
# put the axes on the parametric trajectory
if(todo == "set") {
cmd = sprintf("MOVEP %f %s", p1, addr_list)
ans = _icepap_query(dev, "#", cmd)
if(index(ans, "ERROR")) {
return ".error."
}
# TODO??: wait for the end of motions without using waitmove
# because SPEC does not know the motors that are moving.
# Currently the icepap_traj_sync macro does the job.
}
# always return if we are on trajectory or not
local n addrs[]
local silent
local ret
ret = 1
silent = 1
split(addr_list, addrs)
for(n in addrs) {
addr = addrs[n]
ans = _icepap_query(dev, addr, sprintf("?PARPOS"), silent)
if(index(ans, "ERROR")) {
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
local tt[]
split(motor_par(num, "axis_list"), tt)
icepap_traj_print sprintf("\tissue with : \"%s\"", tt[n])
#icepap_traj_print sprintf("ERROR: \"%s\"", ans)
}
ret = 0
# no sense to not return here but allows to get all motors not sync
#break
}
}
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
icepap_traj_print sprintf("\t%s trajectory", (ret)?"ON":"OUT")
}
return(ret)
}
# put axes on the paramtric trajectory
if (key == "traj_infos") {
if(todo == "set") {
return ".error."
}
local ans
local pmin pmax ipol np
local n addr addrs[]
if((n = split(addr_list, addrs)) < 1) {
return ".error."
}
# check if parametric trajectory is already downloaded and
# then get parameter boundaries for the first motor only
# as we trust that the trajectory has been well loaded
addr = addrs[0]
# answer should be "SPLINE param_beg param_end n_param"
ans = _icepap_query(dev, addr, "?PARDAT", silent)
if(index(ans, "ERROR Uninitialised parametric table")) {
if(!silent) {
_icepap_warn
printf("Uninitialised parametric table for axis \"%s\"\n", \
sprintf("%s:%d", dev, addr))
_icepap_warn
printf("Hint: use \"icepap_traj_load\"\n")
}
return ".error."
}
# get parameter boundaries
if(sscanf(ans, "%s %f %f %d", ipol, pmin, pmax, np) != 4) {
return ".error."
}
ans = sprintf("Tranjectory associated to \"%s\:\n", mne)
ans = sprintf("%s\tnb of points : %d\n", ans, np)
ans = sprintf("%s\tparam max val: %f\n", ans, pmax)
ans = sprintf("%s\tparam min val: %f\n", ans, pmin)
ans = sprintf("%s\tparam max vel: %fHz\n", \
ans, motor_par(num, "max_velocity"))
ans = sprintf("%s\tparam cur vel: %fHz\n", \
ans, motor_par(num, "slew_rate"))
ans = sprintf("%s\treal motors : %s\n", \
ans, motor_par(num, "axis_list"))
ans = sprintf("%s\ttraj array : %s\n", \
ans, motor_par(num, "traj_name"))
return(ans)
}
# return max parameter velocity in Hz
if((key == "max_velocity") && (todo == "get")) {
local i n addrs[] addr mnes[] mn
local vel_max vel
local stpz
# just in case the concerned motor has never been moved by SPEC before
n = split(motor_par(num, "axis_list"), mnes)
for(i=0;i<n;i++){
mn = motor_num(mnes[i])
icepap_cmd(mn, "slew_rate", motor_par(mn, "slew_rate"))
icepap_cmd(mn, "acceleration", motor_par(mn, "acceleration"))
}
stpz = motor_par(num, "step_size")
n = split(addr_list, addrs)
for(i=0, vel_max=1e50;i<n;i++){
addr = addrs[i]
ans = _icepap_query(dev, addr, "?PARVEL MAX")
if(index(ans, "ERROR")) {
return(0)
}
sscanf(ans, "%f", vel)
vel = fabs(vel)
if(vel < vel_max) {
vel_max = vel
}
}
vel_max *= stpz
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
icepap_traj_print sprintf("\tvel value : %fHz", vel_max)
}
return(vel_max)
}
# default return for unknown key get request
return(0)
}'
#%IU%
#%MDESC%
# MACRO MOTOR:
# Print debug messages
#
def icepap_traj_print '{
print "TRAJ MACMOT: " $*
}'
#%IU%(num, key, p1, p2)
#%MDESC%
# MACRO MOTOR:
# Called by spec on motor operation.
#
def icepap_traj_cmd(num, key, p1, p2) '{
global ICEPAP_TRAJ_VERB
local mne
local dev
local cmd ans i
local silent
local addr_list
# Handle actions at controller level
if(num == "..") {
return
}
# get the motor mnemonic string
mne = motor_mne(num)
dev = motor_par(num, "address")
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
icepap_traj_print sprintf("\"%s\" \"%s\"", mne, key)
}
# get the list of real motor addresses
addr_list = motor_par(num, "addr_list")
# return the current motor position in mm or deg
if (key == "position") {
local addrs[]
local addr
local param
if(!motor_par(num, "sync")) {
#_icepap_warn
#printf("\"%s\" not on trajectory\n", mne)
#return ".error."
return(0)
}
split(addr_list, addrs)
addr = addrs[0]
ans = _icepap_query(dev, addr, sprintf("?PARPOS"))
if(index(ans, "ERROR")) {
_icepap_err
printf("\"%s\" position read error\n", mne)
#return ".error."
return(0)
}
sscanf(ans, "%f", param)
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
icepap_traj_print sprintf("\tparam value : %f", param)
}
return(param)
}
# start a motion (p1==abs pos, p2==rel pos, with pos in mm or deg)
if (key == "start_one") {
silent = 1
# TODO: wait for GROUP parsing fix in DSP
#cmd = sprintf("PMOVE GROUP %f %s", p1, addr_list)
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
icepap_traj_print sprintf("\tparam value : %f", p1)
}
cmd = sprintf("PMOVE %f %s", p1, addr_list)
ans = _icepap_query(dev, "#", cmd, silent)
if(i = index(ans, "ERROR")) {
_icepap_err
if(index(ans, "sync needed") || index(ans, "mode is not in sync")) {
printf("\"%s\" not on trajectory\n", mne)
} else {
print substr(ans, i+6)
}
}
}
# return the current motor status
if (key == "get_status") {
local stats[] sta n
local ret
# No summarized information where to know if the parametric
# motion is finished or not. Therefore all concerned physical
# motors have to be checked
cmd = sprintf("?FSTATUS %s", addr_list)
ans = _icepap_query(dev, "", cmd)
if(index(ans, "ERROR")) {
_icepap_err
printf("\"%s\" unable to update its status\n", mne)
return ".error."
}
ret = 0
split(ans, stats)
for(n in stats) {
if(sscanf(stats[n], "%x", sta) != 1) {
_icepap_err
printf("unable to get status\n")
return ".error."
}
# see get_status in standard IcePAP macros for details
if((sta & (1<<10)) || (sta & (1<<11))) ret |= 0x02;
else if(!(sta & (1<<9)) && (sta & (1<<23))) ret |= 0x02;
# no sense to test limitswitches, the parametric motor
# has no hardware limits
# if one motor is moving unable to test the others
if(ret) {
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
local tt[]
split(motor_par(num, "axis_list"), tt)
icepap_traj_print sprintf("\tstill moving: \"%s\"", tt[n])
}
# no sense to not return here but allows to get all motors moving
#return ret
}
}
# TODO: avoid over calling this sync
# NOTE: from "help motors": position discrepancies between spec and the
# motor hardware will be silently resolved in favor of the hardware
read_motors(0x06)
return ret
}
# stop all concerned real motors
if (key == "abort_one") {
cmd = sprintf("STOP %s", addr_list)
_icepap_query(dev, "", cmd)
return
}
# set acceleration (p1=ms p2=steps/sec^2)
if (key == "acceleration") {
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
icepap_traj_print sprintf("\tacct value : %f", p1/1000.0)
}
cmd = sprintf("PARACCT %f %s", p1/1000.0, addr_list)
ans = _icepap_query(dev, "#", cmd)
return
}
# set slew_rate (p1=rate in Hz)
if (key == "slew_rate") {
# Do not set PARVEL if no trajectory has been loaded for each real axis
# to avoid errors on PARVEL boundaries
local stpz
stpz = motor_par(num, "step_size")
if(ICEPAP_TRAJ_VERB & ICEPAP_TRAJ_VMACMOT) {
icepap_traj_print sprintf("\tvel value : %f", p1/stpz)
}
cmd = sprintf("PARVEL %f %s", p1/stpz, addr_list)
ans = _icepap_query(dev, "#", cmd)
if(index(ans, "ERROR Uninitialised parametric table")) {
return
}
if(index(ans, "ERROR")) {
return ".error."
}
}
}'
#%MACROS%
#%IMACROS%
#%AUTHOR% MP BLISS (Original 02/2015).
# %BR%$Revision: 1.6 $ / $Date: 2020/11/20 17:19:32 $
#%TOC%
|
