#%TITLE% TAB3.MAC
#%NAME%
# Macros to control three leg table(s) with logical motors
# xtilt, ytilt and height
#%DESCRIPTION%
# A table with three legs in triangle. One in the front and two in the
# back is a common positioning element in ESRF beamlines. This macros
# implement tilts and height for this kind of table as pseudomotors.
# This macros can use different geometries.
# More than one table can be defined in this way in the same spec
# application.%BR%
#
# For geometry 0 (standard):%BR%
# Xtilt is the angle defined by the two back legs.
# Fixed a middle point B between the two back legs. The height in the
# middle point between B and the front leg gives the value for the
# motor height. The motor Ytilt is defined as the angle existing
# between B and front leg.
#
# Two parameters must be input to the system: the distance between the
# two back legs and the distance between B and the front leg.
# In this geometry distances are given in mm. Angles in mrad.%BR%
# For geometry 1 (side front leg):%BR%
# The front leg is on the same side and aligned with the first motor.
# The fixed point is in the middle of the mirror. For positive x tilt
# is motor 1 higher up then motor 2. For positive y tilt is the
# front motor higher up then the two back motors.%BR%
# For geometry 2 (ID22 mirror):%BR%
# Xtilt is the angle defined by the two back legs, but only leg2 moves
# when a xtilt is requested. Ytilt and Height are defined in the line
# defined by leg1 and front. For the two movements leg2 follows leg1.%BR%
# For geometry 3 (ID21 mirror):%BR%
# Xtilt is the angle defined by the two back legs.
# Fixed a middle point B between the two back legs. The value of this
# point gives the value for the motor Height. The motor Ytilt is
# defined as the angle existing between B and front leg.
# Two parameters must be input to the system: the distance between the
# two back legs and the distance between B and the front leg.
# In this geometry distances are given in mm. Angles in mrad.%BR%
# For geometry 4 (ID20):%BR%
# For geometry 5 (ID29 mirror):%BR%
# Xtilt is the angle defined by the two back legs.
# B is the middle distance point between the two back legs. C is at
# fixed distance between B and the front leg, used to calculate the
# height. The Ytilt is defined as the angle between B and the front leg.
# Three parameters needed: the distance between the two back legs,
# between B and the front leg and between C and the front leg (all
# given in mm). Calculated angles in mrad.%BR%
# For geometry 6 (ID30):%BR%
#
# All motors must be defined in config.
#
global tab3prefix
tab3prefix = "tab3"
def tab3_method(id, method, par1) '{
if ( PROPERTIES["mnemonic"][id] != tab3prefix ) {
print "Unknown " tab3prefix " " id
return 0
}
if (method == "getconfig") {
conf = PROPERTIES[id]["motors"]
conf = sprintf("%s %s %s %s %s %s", \
motor_mne(PROPERTIES[id]["chan0"]), \
motor_mne(PROPERTIES[id]["chan1"]), \
motor_mne(PROPERTIES[id]["chan2"]), \
PROPERTIES[id]["motors"], \
PROPERTIES[id]["d"], \
PROPERTIES[id]["D"] )
return conf
} else {
print "Unknown method for tab3 " method
return "Not ok"
}
}'
def tab3_config(motormne, type, par1,par2) '{
if ( type == "ctrl") {
load_properties()
}
if ( type == "mot" ) {
return tab3_motconfig(motormne,par1,par2)
}
}'
def tab3_calc(mne, mode) '{
local id unit chan motnum small_d big_d
local lb1num lb2num lfnum
local p pp[]
local d1, d2, d3
if (mne == "..") {
return
}
if ( mode == 0 ) {
motnum = motor_num(mne)
chan = motor_par(motnum,"channel")
unit = int(chan/10)
chan = chan%10
pp = get_properties( get_ctrl_id( tab3prefix, unit) )
lb1num = pp["lb1num"]
lb2num = pp["lb2num"]
lfnum = pp["lfnum"]
hgtnum = motor_num( pp["chan0"])
xtnum = motor_num( pp["chan1"])
ytnum = motor_num( pp["chan2"])
d1 = pp["d"]
d2 = pp["D"]
d3 = pp["center"]
geo = pp["geo"]
t3_bl1 = A[lb1num]
t3_bl2 = A[lb2num]
t3_fro = A[lfnum]
t3_xti = A[xtnum]
t3_yti = A[ytnum]
t3_hgt = A[hgtnum]
if (chan == 0) { # height
A[motnum] = tab3_tf( 5, t3_bl1, t3_bl2, t3_fro, t3_xti, t3_yti, t3_hgt, geo, d1,d2,d3)
} else if (chan == 1) { # xtilt
A[motnum] = tab3_tf( 3, t3_bl1, t3_bl2, t3_fro, t3_xti, t3_yti, t3_hgt, geo, d1,d2,d3)
} else if (chan == 2) { # ytilt
A[motnum] = tab3_tf( 4, t3_bl1, t3_bl2, t3_fro, t3_xti, t3_yti, t3_hgt, geo, d1,d2,d3)
}
} else {
pp = get_properties( get_property(mne, tab3prefix))
motnum = motor_num(mne)
lb1num = pp["lb1num"]
lb2num = pp["lb2num"]
lfnum = pp["lfnum"]
hgtnum = motor_num( pp["chan0"])
xtnum = motor_num( pp["chan1"])
ytnum = motor_num( pp["chan2"])
d1 = pp["d"]
d2 = pp["D"]
d3 = pp["center"]
geo = pp["geo"]
t3_bl1 = A[lb1num]
t3_bl2 = A[lb2num]
t3_fro = A[lfnum]
t3_xti = A[xtnum]
t3_yti = A[ytnum]
t3_hgt = A[hgtnum]
if ( motnum == -1 ) {
print "Mne " mne ".Motnum: " motnum
} else if ( mne == pp["lb1num"]) {
A[motnum] = tab3_tf( 0, t3_bl1, t3_bl2, t3_fro, t3_xti, t3_yti, t3_hgt, geo, d1,d2,d3)
} else if ( mne == pp["lb2num"]) {
A[motnum] = tab3_tf( 1, t3_bl1, t3_bl2, t3_fro, t3_xti, t3_yti, t3_hgt, geo, d1,d2,d3)
} else if ( mne == pp["lfnum"]) {
A[motnum] = tab3_tf( 2, t3_bl1, t3_bl2, t3_fro, t3_xti, t3_yti, t3_hgt, geo, d1,d2,d3)
}
}
}'
def tab3_ctrlconfig(unit) '{
local id lb1mne lb2mne lfmne
local lb1num lb2num lfnum
id = get_ctrl_id( tab3prefix,unit)
print "Controller configuration for " id
lb1mne = get_property(id,"back1")
lb2mne = get_property(id,"back2")
lfmne = get_property(id,"front")
lb1num = motor_num(lb1mne)
lb2num = motor_num(lb2mne)
lfnum = motor_num(lfmne)
if ( (lb1num == -1) || (lb2num == -1) || (lfnum == -1)) {
printf("Missing motors for TAB3, Unit %s -> (%s,%s,%s)\n",unit,lb1mne,lb2mne,lfmne)
} else {
set_property( id, "lb1num", lb1num)
set_property( id, "lb2num", lb2num)
set_property( id, "lfnum", lfnum)
set_property( id, "motors", sprintf("%s %s %s",lb1mne,lb2mne,lfmne))
set_property( lb1num, tab3prefix, id)
set_property( lb2num, tab3prefix, id)
set_property( lfnum, tab3prefix, id)
ids = get_property(tab3prefix,"ids")
set_property( id, "ready", 1)
ctrl_register(id,tab3prefix)
}
}'
def tab3_motconfig(mne,unit,chan) '{
chan = motor_par(motor_num(mne),"channel")
unit = int(chan/10)
chan = chan%10
id = get_ctrl_id(tab3prefix,unit)
if (!get_property(id,"ready")) {
tab3_ctrlconfig(unit)
}
set_property(id, sprintf("chan%s",chan), motor_num(mne))
return get_property(id, "motors")
}'
#%IU% (calcidx , tbl1, tbl2, front, xti, yti, z, mode , d1, d2,d3)
#%MDESC% calcidx gives the index of the value to be calculated. Indexes are
# starting from 0 (tbl1) and go to 5 (z). The 3 leg motors and the 3 pseudo
# motors are all the time given. Depending on calcidx only 3 of them are used
# in the calculation. The mode allows for different geometries. d1 and d2 are
# the distances between the legs. d3 is the distance between the front leg
#and the center of the rotation.
def tab3_tf(calcidx,t3_bl1,t3_bl2,t3_fro,t3_xti,t3_yti,t3_hgt,mode,d1,d2,d3) '{
local reverse res
if (calcidx > 2 )
reverse = 1
if (mode == 0) {
if (reverse) {
res[3] = 1000*atan( (t3_bl2 - t3_bl1) / d1 ) # xti
res[4] = 1000*atan( ( (t3_bl1 + t3_bl2)/2 - t3_fro) / d2) # yti
res[5] = (t3_bl1 + t3_bl2 + 2*t3_fro)/4 # z
} else {
res[0] = t3_hgt - d1 * tan(t3_xti/1000) / 2+d2*tan(t3_yti/1000)/2 # tbl 1
res[1] = t3_hgt + d1 * tan(t3_xti/1000) / 2+d2*tan(t3_yti/1000)/2 # tbl 2
res[2] = t3_hgt - d2 * tan(t3_yti/1000) / 2 # front
}
}
if (mode == 1) {
if (reverse) {
res[3] = 1000*atan( (t3_bl2 - t3_bl1) / d1 ) # xti
res[4] = 1000*atan( (t3_fro - t3_bl1) / d2 ) # yti
res[5] = (t3_bl2 + t3_fro)/2 # z
} else {
res[0] = t3_hgt - d1 * tan(t3_xti/1000) / 2 - d2*tan(t3_yti/1000) / 2 # tbl 1
res[1] = t3_hgt + d1 * tan(t3_xti/1000) / 2 - d2*tan(t3_yti/1000) / 2 # tbl 2
res[2] = t3_hgt + d2 * tan(t3_yti/1000) / 2 - d1*tan(t3_xti/1000) / 2 # front
}
}
if (mode == 2) {
if (reverse) {
res[3] = deg(atan( (t3_bl2 - t3_bl1) / d1 )) # xti
res[4] = deg(atan( (t3_bl1 - t3_fro) / d2 )) # yti
res[5] = (t3_bl1 + t3_fro)/2 # z
} else {
res[0] = t3_hgt + d2 * tan(rad(t3_yti))/2 # tbl 1
res[1] = t3_hgt + d2 * tan(rad(t3_yti))/2 + d1 * tan(rad(t3_xti)) # tbl 2
res[2] = t3_hgt - d2 * tan(rad(t3_yti))/2 # front
}
}
if (mode == 3) {
if (reverse) {
res[3] = 1000*atan( (t3_bl2 - t3_bl1) / d1 ) # xti
res[4] = 1000*atan( ( (t3_bl1 + t3_bl2)/2 - t3_fro) / d2) # yti
res[5] = (t3_bl1 + t3_bl2)/2 # z
} else {
res[0] = t3_hgt - d1 * tan(t3_xti/1000) / 2 # tbl 1
res[1] = t3_hgt + d1 * tan(t3_xti/1000) / 2 # tbl 2
res[2] = t3_hgt - d2 * tan(t3_yti/1000) # front
}
}
if (mode == 4) {
if (reverse) {
res[3] = 1000*atan( (t3_bl2 - t3_bl1) / d1 ) # xti
res[4] = 1000*atan( ( t3_fro - (t3_bl1 + t3_bl2)/2 ) / d2) # yti
res[5] = (t3_bl1 + t3_bl2 + 2*t3_fro)/4 # hgt
} else {
res[0] = t3_hgt - d1 * tan(t3_xti/1000) / 2 - d2 * tan(t3_yti/1000) / 2 # tbl 1
res[1] = t3_hgt + d1 * tan(t3_xti/1000) / 2 - d2 * tan(t3_yti/1000) / 2 # tbl 2
res[2] = t3_hgt + d2 * tan(t3_yti/1000) / 2 # front
}
}
if (mode == 5) {
if (reverse) {
res[3] = 1000*atan( (t3_bl2 - t3_bl1) / d1 ) # xti
res[4] = 1000*atan( ( (t3_bl1 + t3_bl2)/2 - t3_fro) / d2) # yti
res[5] = t3_fro + ( (t3_bl1 + t3_bl2)/2 - t3_fro) * (d3/d2) #z
} else {
res[0] = t3_hgt - d1 * tan(t3_xti/1000) / 2 + (d2 - d3)*tan(t3_yti/1000) #tbl 1
res[1] = t3_hgt + d1 * tan(t3_xti/1000) / 2 + (d2 - d3)*tan(t3_yti/1000) #tbl 2
res[2] = t3_hgt - d3 * tan(t3_yti/1000) #front
}
}
if (mode == 6) {
if (reverse) {
res[3] = deg( atan( (t3_bl2 - t3_bl1) / d1 ) ) # xti
res[4] = deg( atan( ( (t3_bl1 + t3_bl2)/2 - t3_fro) / d2) ) # yti
res[5] = (t3_bl1 + t3_bl2) / 2 # z
} else {
res[0] = t3_hgt - d1 * tan( rad(t3_xti) ) / 2 # tbl 1
res[1] = t3_hgt + d1 * tan( rad(t3_xti) ) / 2 # tbl 2
res[2] = t3_hgt - d2 * tan( rad(t3_yti) ) # front
}
}
return res[calcidx];
}'
#%MACROS%
#%IMACROS%
#%DEPENDENCIES%
#%AUTHOR% Vicente Rey. June 1995
#$Revision: 1.1 $, $Date: 2011/06/11 07:42:26 $
#%TOC%
|
