#%TITLE% t2mh.MAC
#%NAME%
# Macros for operating tables with 2 independent legs.
# Can be used for any 2 actuators device (ex : mirror)
#
#%PRE%
# This macro set allows you to define macros motors on tab2s.
#
# Angles are considered smalls and in miliradians.
#
# a tab2 controller in config (one controller will work on one
# pair of legs)
# MOTORS DEVICE ADDR <>MODE NUM <>TYPE
# YES tab2 2 Macro Motors
#
# motors in config referring to that controller. Channel assignment
# will decide on the motor role as follow:
#
# Channel
# 0 - theta angle
# 1 - offset
#
# The reference point for rotation and offset is defined via the 'lup'
# and 'ldown' properties.
#
# Different pairs of tab2s can be defined by using additional
# module numbers in the motor unit/module/channel assignment.
#
# Config file
# The following motor parameters have to be associated in config
# file to the channel 0 of the controller/module.
#
# Example:
#
# MOT000 = MAXE:0/0 [...] motup
# MOT001 = MAXE:0/1 [...] motdwn
# MOT006 = MAC_MOT:0/0/0 [....] tht
# MOTPAR:legup = motup
# MOTPAR:legdown = motdwn
# MOTPAR: lup = 30
# MOTPAR: ldown = 50
# MOTPAR: back_opp = 1 new new
# MOT007 = MAC_MOT:0/0/1 [....] offset
#
# Note: requested by ID26: 3/5/17 on the microjacks
# If back_opp is defined (with any value <>0), that means that
# legdown will reverse its config backlash value when a motion on tht is required.
# On all other motions, tht will keep its config backlash value
#
#%PRE%
#%OVERVIEW%
global tab2_DEBUG
######################################################################
############################ ###########################
############################ MACRO MOTOR ###########################
############################ ###########################
######################################################################
#%IU% (<mot_num>, <type>, <unit>, <module>, <chan>)
#%MDESC%
# calc macromotor: config
def tab2_config(mot_num, type, unit, module, chan) '{
local master mot_mne
local leg_up leg_down
local lup ldown
local back_opp
mot_mne = motor_mne(mot_num)
__tab2_debug ">>> tab2_config: ", mot_num, type, unit, module, chan
if (type == "ctrl"){
return(0)
}
master = motor_par(mot_num, "chan0")
leg_up = motor_par(master, "legup")
leg_down = motor_par(master, "legdown")
lup = motor_par(master, "lup")
ldown = motor_par(master, "ldown")
back_opp = motor_par(master, "back_opp")
__tab2_debug ">>> tab2_config: master: ", master
__tab2_debug ">>> tab2_config: legup: ", leg_up
__tab2_debug ">>> tab2_config: legdown:", leg_down
__tab2_debug ">>> tab2_config: lup: ", lup
__tab2_debug ">>> tab2_config: ldown: ", ldown
__tab2_debug ">>> tab2_config: back_opp: ", back_opp
if ( motor_num(leg_up) == -1 || motor_num(leg_down) == -1) {
print "tab2: motor " leg_up " or motor " leg_down " missing. exit"
return(".error.")
}
motor_par(mot_num, "master", master, "add")
if (chan == 0) {
__tab2_debug ">>> tab2_config: on chan: ", chan
motor_par(mot_num, "role", "theta", "add")
motor_par(mot_num, "locked", 0, "add")
motor_par(mot_num, "setp", -999, "add")
motor_par(mot_num, "oldpos", -999, "add")
motor_par(master, "theta", mot_mne, "add")
motor_par(master, "lup", lup, "add")
motor_par(master, "ldown", ldown, "add")
motor_par(leg_up, "master", mot_num, "add")
motor_par(leg_down, "master", mot_num, "add")
motor_par(leg_up, "oldpos", -999, "add")
motor_par(leg_down, "oldpos", -999, "add")
motor_par(leg_up, "role", "legup", "add")
motor_par(leg_down, "role", "legdown", "add")
} else if (chan == 1) {
__tab2_debug ">>> tab2_config: on chan: " chan
motor_par(mot_num, "role", "tab2off", "add")
motor_par(mot_num, "locked", 0, "add")
motor_par(mot_num, "setp", -999, "add")
motor_par(mot_num, "oldpos", -999, "add")
motor_par(master, "tab2off", mot_mne, "add")
} else {
print "Wrong channel assignment for tab2 motor. Only 0 or 1 accepted"
return(".error.")
}
cdef("user_checkall", sprintf("tab2_checkmove(%s);\n", master), motor_mne(master), 0x01)
# Returns motor dependance list.
return(sprintf("%s %s", leg_up, leg_down))
}'
#%IU%(<mot_num>, <mode>, <master0>)
#%MDESC% calc macromotor: calc
def tab2_calc(mot_num, mode, master0) '{
local role master
local leg_up leg_down
local tab2off theta lup ldown
local leg_up_num leg_down_num
local offnum thetanum thetapos
local radval degval
__tab2_debug ">>> tab2_calc: ", mot_num, mode, master0
if (mot_num == "..") {
return(0)
}
role = motor_par(mot_num, "role")
master = motor_par(mot_num, "master")
__tab2_debug ">>> tab2_calc: motor to calc: ", mot_num
__tab2_debug ">>> tab2_calc: on mode: ", mode
__tab2_debug ">>> tab2_calc: role: ", role
if ( mode == 0) {
# Calculates theta and offset.
leg_up = motor_par(master, "legup")
leg_down = motor_par(master, "legdown")
lup = motor_par(master, "lup")
ldown = motor_par(master, "ldown")
leg_up_num = motor_num(leg_up)
leg_down_num = motor_num(leg_down)
if ((lup+ldown) < 1.0e-8) {
print "bad tab2 lengths values. exit"
print "Space between 2 leg too small"
return(".error.")
}
if (role == "tab2off") {
A[mot_num] = (lup * (A[leg_down_num]-A[leg_up_num]) / (lup+ldown)) + A[leg_up_num]
}
else if ( role == "theta") {
# radval = atan( (A[leg_down_num] - A[leg_up_num])/(lup+ldown) )
# degval = deg(radval)
# A[mot_num] = degval
# in fact, small angles and millirad
A[mot_num] = (A[leg_down_num] - A[leg_up_num])/(lup+ldown) * 1000
}
}
else {
# Calculates leg1 and leg2.
tab2off = motor_par(master, "tab2off")
theta = motor_par(master, "theta")
lup = motor_par(master, "lup")
ldown = motor_par(master, "ldown")
offnum = motor_num(tab2off)
thetanum = motor_num(theta)
__tab2_debug ">>> tab2_calc: role: ", role
__tab2_debug ">>> tab2_calc: offnum: ", offnum
__tab2_debug ">>> tab2_calc: thetanum: ", thetanum
offpos = A[offnum]
thetapos = A[thetanum]
#degval = A[thetanum]
#radval = rad(degval)
__tab2_debug ">>> tab2_calc: z theta: ", offpos, thetapos
if ( motor_par(offnum, "locked") ) {
offpos = motor_par(offnum, "setp")
}
if ( motor_par(thetanum,"locked") ) {
thetapos = motor_par(thetanum,"setp")
}
# if (role == "legup") {
# A[mot_num] = offpos - lup*tan(radval)
# } else if (role == "legdown") {
# A[mot_num] = offpos + ldown*tan(radval)
# }
# small angles and millirad, so..
if (role == "legup") {
A[mot_num] = offpos - lup*thetapos/1000
} else if (role == "legdown") {
A[mot_num] = offpos + ldown*thetapos/1000
}
}
__tab2_debug ">>> tab2_calc: returns: ", A[mot_num]
#__tab2_debug ">>> tab2_calc: motor: ",mot_num, " set to ", A[mot_num]
motor_par( mot_num,"oldpos", A[mot_num])
}'
######################################################################
############################### ##############################
############################### MISC ##############################
############################### ##############################
######################################################################
# if macromotor is replaced by NONE, then some elements become -1
# but user_checkall remains, so .... need to protect the motor_par access
# ex: when theta becomes NONE
#>>> tab2_checkmove: 12
#>>> tab2_checkmove: theta: -1
#>>> tab2_checkmove: off: -1
#>>> tab2_checkmove: legup: 9
#>>> tab2_checkmove: legdown: 8
#>>> tab2_checkmove: offnum: -1
#>>> tab2_checkmove: thetanum: -1
#>>> tab2_checkmove: now on legdown 8
#>>> tab2_checkmove: now on legup 9
#>>> tab2_checkmove: now on off -1
#>>> tab2_checkmove: now on theta -1
#%IU%(master)
#%MDESC%
# To lock positions for calc. Provide as parameter a tab2 master
#(the one at channel 0).
def tab2_checkmove(master) '{
local tab2mots
local diff mot mot_num
__tab2_debug ">>> tab2_checkmove: ",master
tab2mots["theta"] = motor_num( motor_par(master,"theta") )
tab2mots["off"] = motor_num( motor_par(master,"tab2off") )
tab2mots["legup"] = motor_num( motor_par(master,"legup") )
tab2mots["legdown"] = motor_num( motor_par(master,"legdown") )
offnum = tab2mots["off"]
thetanum = tab2mots["theta"]
__tab2_debug ">>> tab2_checkmove: theta:" , tab2mots["theta"]
__tab2_debug ">>> tab2_checkmove: off: " , tab2mots["off"]
__tab2_debug ">>> tab2_checkmove: legup: " , tab2mots["legup"]
__tab2_debug ">>> tab2_checkmove: legdown: " , tab2mots["legdown"]
__tab2_debug ">>> tab2_checkmove: offnum: " , offnum
__tab2_debug ">>> tab2_checkmove: thetanum: ", thetanum
for ( mot in tab2mots ) {
mot_num = tab2mots[mot]
__tab2_debug ">>> tab2_checkmove: now on " mot " " mot_num
if (mot_num != -1) {
diff = fabs( A[mot_num] - motor_par(mot_num, "oldpos") )
__tab2_debug ">>> tab2_checkmove: for motion of " diff
if ( diff > 1.0 / motor_par( mot_num, "step_size") ) {
if ( mot == "legup" || mot == "legdown") {
if ((tab2mots["theta"] != -1) && (tab2mots["off"] != -1)) {
__tab2_debug ">>> tab2_checkmove: unlock theta and off"
motor_par( tab2mots["theta"], "locked", 0)
motor_par( tab2mots["off"], "locked", 0)
}
if (motor_par(master,"back_opp") != 0) {
motor_par( tab2mots["legdown"],"backlash",motor_par( tab2mots["legdown"],"config_backlash"))
__tab2_debug ">>> tab2_checkmove: backlash legdown"
}
} else {
if ( mot == "off" ) { # offset - fix theta
if (thetanum != -1) {
__tab2_debug ">>> tab2_checkmove: lock theta"
if ( motor_par(thetanum,"locked") == 0 ) {
motor_par(thetanum,"setp", A[thetanum])
motor_par(thetanum,"locked", 1)
}
}
if (motor_par(master,"back_opp") != 0) {
motor_par( tab2mots["legdown"],"backlash",motor_par( tab2mots["legdown"],"config_backlash"))
__tab2_debug ">>> tab2_checkmove: backlash legdown"
}
} else { # theta - fix offset
__tab2_debug ">>> tab2_checkmove: lock offset"
if (offnum != -1) {
if ( motor_par(offnum, "locked") == 0 ) {
motor_par(offnum, "setp", A[offnum])
motor_par(offnum, "locked", 1)
}
}
if (motor_par(master,"back_opp") != 0) {
motor_par( tab2mots["legdown"],"backlash",motor_par( tab2mots["legdown"],"config_backlash")*(-1))
__tab2_debug ">>> tab2_checkmove: backlash legdown * (-1)"
}
}
motor_par(mot_num, "locked", 0)
}
}
}
}
}'
#%UU% <mne>
#%MDESC%
# Ask for infos on a tab2 set.
# Provide as argument one of the calc motors.
def tab2 '{
local mne master
mne = "$1"
__tab2_debug ">>> tab2: ", mne
master = motor_par(motor_num(mne), "chan0")
__tab2_debug ">>> tab2: master: ", master
print "theta : " motor_par(master, "theta")
print "Offset : " motor_par(master, "tab2off")
print "legup : " motor_par(master, "legup")
print "legdown : " motor_par(master, "legdown")
}'
#%UU%
#%MDESC%
# Toggles the debug mode.
if (!(whatis("__tab2_debug") & 2)) rdef __tab2_debug \'#$*\'
def tab2_debug '{
if ((whatis("__tab2_debug")>>16) <= 3) { # macro length is 3 bytes: comment
rdef __tab2_debug "eprint"
print "tab2 macro debug mode is ON"
tab2_DEBUG = 1
}
else {
rdef __tab2_debug \'#\$*\'
print "tab2 macro debug mode is OFF"
tab2_DEBUG = 0
}
}'
#%MACROS%
#%IMACROS%
#%LOG%
#$Revision: 1.4 $
#$Log: t2mh.mac,v $
#Revision 1.4 2017/08/10 13:24:57 domingue
#add MOTPAR back_opp to be able to reverse backlash of legdown motor when moving legup and legdown in opposite direction
#
#Revision 1.3 2013/07/17 15:05:18 guilloud
#*cosmetic refactoring+doc
#
#Revision 1.1 2011/09/26 08:43:19 domingue
#Initial revision
#
#%AUTHOR%
# mcd july 2011
#%TOC%
|
