#%TITLE% STACKED.MAC
#%NAME%
# Macros for stacked pseudomotors.
#
#%OVERVIEW%
# A stacked pseudomotor is a combination of a real motor and piezoelectric actuator that drive the same axis. The motor and piezo are used as coarse and fine adjustments of the position of the axis. The whole axis is described by a pseudomotor `combined', while two other motors `coarse' and `fine' represent the two independent adjustments.%BR%
# Three different modes of operation (OFF, ON and AUTO) are provided by this macro set. The modes ON and AUTO require a high resolution encoder that provides the position of the combined axis. The particularities of modes of operation are the following: %BR%
#%DL%
#%DT% OFF %DD% Mode off
#%DT% ON
#%DD% Mode on
#%DT% AUTO
#%DD% Mode auto
#%XDL%
#
#
#%EXAMPLE%
#
# %B%%B%%BR%
#
#%IU% axis coarse fine combined mode threshold piezodev calibP [encoderdev calibE]
#%MDESC%
# Loads configuration parameters in the main global variable.
#
def stackedput '{
local i
i = 12*($1)
STCKPAR[i+1] = $2
STCKPAR[i+2] = "$2"
STCKPAR[i+3] = $3
STCKPAR[i+4] = "$3"
STCKPAR[i+5] = $4
STCKPAR[i+6] = "$4"
STCKPAR[i+7] = $5
STCKPAR[i+8] = $6
STCKPAR[i+9] = "$7"
STCKPAR[i+10] = $8
STCKPAR[i+11] = "$9"
STCKPAR[i+12] = $10
}'
#%IU% axis mode_variable
#%MDESC%
def stackedmodeget '$2 = STCKPAR[12*($1)+7]'
#%IU% axis newmode
#%MDESC%
def stackedmodeput 'STCKPAR[12*($1)+7] = ($2)'
#%IU% axis
#%MDESC%
def stackedget '
local coarse fine combined piezodev encoderdev threshold
coarse = STCKPAR[12*($1)+1]
coarse_mne = STCKPAR[12*($1)+2]
fine = STCKPAR[12*($1)+3]
fine_mne = STCKPAR[12*($1)+4]
combined = STCKPAR[12*($1)+5]
combined_mne = STCKPAR[12*($1)+6]
opmode = STCKPAR[12*($1)+7]
threshold = STCKPAR[12*($1)+8]
piezodev = STCKPAR[12*($1)+9]
calibP = STCKPAR[12*($1)+10]
encoderdev = STCKPAR[12*($1)+11]
calibE = STCKPAR[12*($1)+12]
'
#%UU%
#%MDESC%
def stackedshow '{
if (STCKPAR[0]) {
print " # Coarse Fine Comb Mode Thr." \
" ____Piezo____ CalibP _____Encoder_____ CalibE"
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
printf("%2d %5s %4s %4s %4s %3.2f %13s %7g %17s %7g\n", i+1, \
coarse_mne, fine_mne, combined_mne, _stackedmode, \
threshold, piezodev, calibP, encoderdev, calibE)
}
} else {
print "No stacked pseudomotors defined."
}
}'
#%IU%
#%MDESC%
#
def _stackedmode '(opmode>=0? (opmode? (opmode>1?"AUTO":"ON"):"OFF"):"BAD")'
#%UU% [AUTO | ON | OFF [axis]]
#%MDESC%
#
# Lets the user change the operation mode of the stacked axis. In the mode `ON' the position of the axis is determined from the encoder. In the mode `OFF' it is calculated from the voltage applied to the piezo.%BR%
# The names `ON' and `OFF' for the operation modes are not very convenient and should be changed in the future to something meaningful.%BR%
# The value `BAD' is not an operation mode, it indicates that an error was detected during the configuration of the respective axis.
def stackedmode '{
local opmode newmode
if ($# > 2) {
print "Usage: stackedmode [newmode [axis]]"
exit
}
if ($# == 2) {
axis = ($2)
} else {
if (STCKPAR[0] <= 1)
axis = STCKPAR[0]
else
axis = getval("Axis number", sprintf("1-%d", STCKPAR[0])) + 0
}
if (axis <= 0 || axis > STCKPAR[0]){
print "No changes"
exit
}
stackedmodeget axis-1 opmode
opmode = _stackedmode
if ($# > 0) {
newmode = "$1"
} else {
newmode = getval("New mode [ON, OFF or AUTO] ", opmode)
}
if (newmode == "auto" || newmode == "AUTO" || newmode == 2) {
stackedmodeput axis-1 2
} else if (newmode == "on" || newmode == "ON" || newmode == 1) {
stackedmodeput axis-1 1
} else {
stackedmodeput axis-1 0
}
printf("Old mode for stacked axis %d was %s. ", axis, opmode)
stackedconfig
stackedmodeget axis-1 opmode
printf("Now is %s\n", _stackedmode)
}'
#%UU% [axis_number]
#%MDESC%
#
def onstacked 'stackedmode ON $*'
#%UU% [axis_number]
#%MDESC%
#
def offstacked 'stackedmode OFF $*'
#%UU% [axis_number]
#%MDESC%
#
def autostacked 'stackedmode AUTO $*'
#%UU% [axis_number]
#%MDESC%
#
def stackedoffset '{
local axis volt
if ($# > 2) {
print "Usage: stackedoffset [axis]"
exit
}
if ($# == 1) {
axis = ($1)
} else {
if (STCKPAR[0] <= 1)
axis = STCKPAR[0]
else
axis = getval("Axis number", sprintf("1-%d", STCKPAR[0])) + 0
}
if (axis <= 0 || axis > STCKPAR[0]){
print "No changes"
exit
}
if (yesno("Change the encoder register", 0)){
local enc
waitmove; get_angles
stackedget axis-1
ESRF_ERR=-1
volt = esrf_io(piezodev, "DevReadValue")
if (ESRF_ERR) {
printf("Error reading piezo device `%s\'.\nCommand aborted.\n", piezodev)
exit
}
enc = esrf_io(encoderdev, "DevReadCount")
printf("Old value was %g (%g steps), ", enc*calibE, enc)
esrf_io(encoderdev, "DevWriteCount", (A[coarse]+(volt-5)*calibP)/calibE )
enc = esrf_io(encoderdev, "DevReadCount")
printf("new value is %g (%g steps).", enc*calibE, enc)
}
}'
#%UU% coarse fine combined mode threshold piezo_dev calibP [encoder_dev calibE]
#%MDESC%
# Configures a stacked axis. `coarse', `fine' and `combined' are the mnemonics of the motor that constitute the axis.%BR%
# `mode' is the default operation mode for this axis. Valid operation modes are 0, 1 or 2 (OFF, ON and AUTO). `mode' is a default value, if the axis was already defined the current value is used and `mode' is ignored.%BR%
# `threshold' is a parameter from 0 to 1 that indicates the range that ... %BR%
# `piezo_dev' and `encoder_dev' are the names of the devices associated to the DAC that drives the piezo and the channel of the encoder respectively. The values `calibP' and `calibE' are the multiplicative factors that convert from piezo and encoder units to motor units.
def stackedsetup '{
global STCKPAR STCKPOSCOARSE STCKPOSFINE STCKPOSCOMB
global STCKVAL STCKSTAT STCKOFF STCKVSTEP STCKDBG
local n
if ($# != 7 && $# != 9) {
if (SETUP) print "Error in line: $0 $*"
print "Usage: "\
"stakedsetup coarse fine comb. mode threshold piezo calP [encoder calE]"
exit
}
n=oldmode=-1
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
if (coarse == $1){
n = i
oldmode = opmode
}
}
if (n < 0) n = (STCKPAR[0]++)
stackedput n $* "" 1
if (oldmode >= 0) {
stackedmodeput n oldmode
}
cdef("user_config", "stackedconfig\n", "stacked")
cdef("user_checkall", "stackedcheckall\n", "stacked")
cdef("user_finished1", "stackedfinished\n", "stacked")
cdef("user_getpangles", "stackedgetangles\n", "stacked")
stackedconfig 0
setup_tail("stacked", "$1")
}'
#%IU% coarse
#%MDESC%
def stackedunsetup '{
local n i j
n = -1
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
if (coarse == $1)
n = i
}
if (n >= 0) {
for (i=n; i<STCKPAR[0]; i++){
for (j=1; j<=12; j++)
STCKPAR[12*i+j] = STCKPAR[12*(i+1)+j]
}
STCKPAR[0]--
}
if (!STCKPAR[0]){
unglobal STCKPAR STCKPOSCOARSE STCKPOSFINE STCKPOSCOMB
unglobal STCKVAL STCKSTAT STCKOFF STCKVSTEP STCKDBG
cdef("", "", "stacked", "delete")
}
}'
#%IU% [dummy_parameter]
#%MDESC%
# Configures all the stacked axis. If an error is found in the configuration of an axis, the mode is st to `BAD' (-1) and a message is printed. If the macro is invoked whit a parameter messages are supressed. %BR%
def stackedconfig '{
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
if (motor_mne(coarse) != coarse_mne || \
motor_mne(fine) != fine_mne || \
motor_mne(combined) != combined_mne ){
stackedmodeput i -1
if (!$#) printf("Wrong motors for stacked axis #%d.\n", i+1)
continue
}
if (calibP == 0 || opmode == 1 &&(encoderdev == "" || calibE == 0)) {
stackedmodeput i -1
if (!$#) printf("Wrong calibration parameters for stacked axis #%d.\n", i+1)
continue
}
if (threshold >= 1) {
stackedmodeput i -1
if (!$#) printf("Wrong threshold value for stacked axis #%d.\n", i+1)
continue
}
if (threshold <= 0) {
if (!$#) printf("Null threshold for stacked axis #%d. Working in `catastrophe\' mode.\n", i+1)
ESRF_ERR = -1
esrf_io(piezodev, "DevSetValue", 5)
}
# Other tests if needed
}
}'
#%IU%
#%MDESC%
# Checks the movements of the stacked axes. Calculates the final position of the `coarse' and `fine' motors. When a `fine' stage has to be adjusted, this macro sets a flag in STCKSTAT and calculates the goal value. The goal value is stored in STCKVAL and is the desired piezo voltage or encoder position (according to the operation mode).
def stackedcheckall '{
local pos
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
if (opmode == 2)
STCKSTAT[i] = 1
else
STCKSTAT[i] = 0
if (opmode >= 0) {
if (A[combined] != STCKPOSCOMB[i]){
if (STCKDBG) printf("Moving `%s\'\n", motor_mne(combined))
pos = STCKPOSFINE[i] + (A[combined] - STCKPOSCOMB[i])
newvolt = pos/calibP - STCKOFF[i]
if (fabs(newvolt/5) > threshold){
A[coarse] = user(coarse, dial(coarse, A[combined]-STCKOFF[i]*calibP))
}
A[fine] = A[combined] - A[coarse]
STCKSTAT[i] = 1
} else if (A[fine] != STCKPOSFINE[i]){
if (STCKDBG) printf("Moving `%s\'\n", motor_mne(fine))
STCKCOMB[i] = A[combined] = A[fine] + A[coarse]
STCKSTAT[i] = 1
} else if (A[coarse] != STCKPOSCOARSE[i]){
if (STCKDBG) printf("Moving `%s\'\n", motor_mne(coarse))
STCKSTAT[i] = 0
if (mode == 2) {
STCKVAL[i] = esrf_io(piezodev, "DevReadValue")
if (STCKVAL[i] < 0.1) STCKVAL[i] = 0.1
if (STCKVAL[i] > 0.9) STCKVAL[i] = 0.9
}
} else
continue
if (STCKSTAT[i]){
STCKVAL[i] = opmode? int(A[combined]/calibE + 0.5) : \
int(409.6*(A[fine]/calibP-STCKOFF[i]+5)+0.5)/409.6
}
}
}
}'
#%IU%
#%MDESC%
# This macro is executed when all the normal motors have completed their movements, including the `coarse' motors of the stacked axes. The fine stages are ramped to their goal positions by adjusting the voltages in the piezos. The amplitud of the maximum voltage step is STCKVSTEP.
def stackedfinished '{
local n i
local voltage encoder incr coarse_incr
local dbglines
STCKVSTEP = 0.05
coarse_adjust = 0
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
if (!STCKSTAT[i] && opmode < 2) continue
voltage[i] = esrf_io(piezodev, "DevReadValue")
if (threshold <= 0 && mode == 2) coarse_adjust=1
}
if (STCKDBG) {
dbglines = 0
tty_move(0, -1)
printf(" # Motor Mode ___Goal___ _Volts_ ___Steps__ ___dV___\n")
}
n = 1
while(n){
n = 0
if (STCKDBG && dbglines > 0) {
tty_move(0, -dbglines-1)
dbglines = 0
}
if (coarse_adjust) {
wait(1)
getangles
}
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
if (!STCKSTAT[i] && opmode < 2) continue
if (opmode && STCKSTAT[i]) {
encoder = esrf_io(encoderdev, "DevReadCount")
incr = 0.25 * (STCKVAL[i] - encoder) * calibE / calibP
} else {
incr = STCKVAL[i] - voltage[i]
}
if (fabs(incr)>STCKVSTEP){
incr = STCKVSTEP*incr/fabs(incr)
}
if (threshold <= 0)
coarse_incr = (STCKVAL[i] - encoder) * calibE
if (STCKDBG){
if (opmode) {
if (opmode == 2 && !STCKSTAT[i])
goalstr = sprintf("%.5g", STCKVAL[i])
else
goalstr = sprintf("%d", STCKVAL[i])
encstr = sprintf("%d", encoder)
} else {
goalstr = sprintf("%.5g", STCKVAL[i])
encstr = "<not used>"
}
printf(">> %d %5s %4s %10s %7.5g %10s %8.5f",i+1, combined_mne, \
_stackedmode, goalstr, voltage[i], encstr, incr)
tty_cntl("ce")
print
dbglines++
}
if (threshold > 0) {
voltage[i] += incr
ESRF_ERR=-1
esrf_io(piezodev, "DevSetValue", voltage[i])
if (ESRF_ERR) {
if (ESRF_ERR == 55)
printf("Trying to move piezo `%s\' out of range.\n", fine_mne)
else
printf("Device server error when moving piezo `%s\'.\n", fine_mne)
print "Motion aborted."
}
if (!ESRF_ERR && fabs(incr) > 1/4096) n=1
} else {
if (coarse_incr > 1/motor_par(coarse, "step_size")){
A[coarse] += coarse_incr
n=1
}
}
}
if (n && coarse_adjust) move_all
}
}'
#%IU%
#%MDESC%
# Calculates the position of the `fine' and `combined' motors in the stacked axes (`coarse' is always obtained from the motor controller). In mode ON the position of `combined' is read from the encoder and `fine' is calculated as the difference respect `coarse'. In mode OFF `fine' is calculated from the voltage in the piezo and `combined' is obtained as the sum of the other two.
# In mode ON it may be an offsset between the zeros of the `coarse' motor and the encoder, in mode OFF that is not the case.
def stackedgetangles '{
local pos voltage i encoder
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
if (opmode >= 0) {
voltage = esrf_io(piezodev, "DevReadValue")
if (opmode) {
encoder = esrf_io(encoderdev, "DevReadCount")
pos = encoder * calibE - A[coarse]
STCKOFF[i] = pos/calibP - (voltage - 5)
if (opmode == 2 && !STCKSTAT[i]) {
STCKVAL[i]=encoder
}
} else {
pos = (voltage - 5) * calibP
STCKOFF[i] = 0
}
STCKPOSCOARSE[i] = A[coarse]
A[fine] = STCKPOSFINE[i] = pos
A[combined] = STCKPOSCOMB[i] = A[fine] + A[coarse]
chg_dial(fine, A[fine])
chg_offset(fine, A[fine])
set_lim(fine, dial(fine, (-4.9+STCKOFF[i])*calibP), \
dial(fine, (4.9+STCKOFF[i])*calibP))
}
}
}'
#%UU% <dummy parameter>
#%MDESC%
#
def stackeddebug '{
local pos encoder voltage i
if ($#) STCKDBG = !STCKDBG
printf("\nOn line debugging is %s. Use `$0 0\' to switch.\n", STCKDBG?"ON":"OFF")
print
waitall; get_angles
print " # __Coarse_ ____Piezo_(volts)___ Coarse+Piezo ___Encoder_(steps)__"
for (i=0; i<STCKPAR[0]; i++) {
stackedget i
if (opmode >= 0) {
voltage = esrf_io(piezodev, "DevReadValue")
printf("%2d %7g %7g (%7g) %7g ", i+1, A[coarse], (voltage-5)*calibP, voltage, A[coarse]+(voltage-5)*calibP)
ESRF_ERR=-1
encoder = esrf_io(encoderdev, "DevReadCount")
printf("%7g (%7g) ", encoder*calibE, encoder)
print
}
}
}'
#%MACROS%
#%IMACROS%
#%SETUP%
#
#%AUTHOR% P. Fajardo, (Original 8/95).
# $Revision: 3.0 $ / $Date: 1999/03/17 10:12:27 $
#%TOC%
|
