#%TITLE% ICE_HELICAL.MAC
#
#%NAME%
# Macros to implement helical scan using IcePAP motors.
#
#%DESCRIPTION%
# Configure a list of IcePAP motors to track the incoming encoder
# signals, currently connected to the IcePAP MASTER SYNCHRO DB9
# connector.
#
# The axes have to be already configured in SPEC session used.
#
# The axes could be located on different IcePAP systems as long
# as they all receive the encoder signals.
#
# Mandatory standard IcePAP macros
need ice
#%IU%
#%MDESC%
# Simulate helical scan parameters from MxCUBE
#
def ice_helical_init '{
global HELICAL_OSCIL_POS[]
global HELICAL_PARS[]
global HELICAL_OSCIL
HELICAL_OSCIL_POS["1"]["kappa"] = "0"
HELICAL_OSCIL_POS["1"]["kappa_phi"] = "0"
HELICAL_OSCIL_POS["1"]["phi"] = "-88.998828125"
HELICAL_OSCIL_POS["1"]["phix"] = "-0.49995555556"
HELICAL_OSCIL_POS["1"]["phiy"] = "22.3563666667"
HELICAL_OSCIL_POS["1"]["phiz"] = "1.44695556011"
HELICAL_OSCIL_POS["1"]["sampx"] = "-0.16850773336"
HELICAL_OSCIL_POS["1"]["sampy"] = "-0.304450170003"
HELICAL_OSCIL_POS["1"]["zoom"] = "0"
HELICAL_OSCIL_POS["2"]["kappa"] = "0"
HELICAL_OSCIL_POS["2"]["kappa_phi"] = "0"
HELICAL_OSCIL_POS["2"]["phi"] = "-88.998671875"
HELICAL_OSCIL_POS["2"]["phix"] = "-0.49995555556"
HELICAL_OSCIL_POS["2"]["phiy"] = "22.8697"
HELICAL_OSCIL_POS["2"]["phiz"] = "1.43195555556"
HELICAL_OSCIL_POS["2"]["sampx"] = "0.0927003798463"
HELICAL_OSCIL_POS["2"]["sampy"] = "-0.278243071775"
HELICAL_OSCIL_POS["2"]["zoom"] = "0"
HELICAL_OSCIL = 1
HELICAL_PARS["nb_pos"] = 2
HELICAL_PARS["phiy_pos"] = 21.4552555555414
HELICAL_PARS["udiff"] = 0
}'
#%UU%(mnemonics)
#%MDESC%
# Configure the list of IcePAP axes that will have to track the
# incoming encoder signals. The list is given as a string of
# blank seperated mnemonics.
# Ex: ice_helical_config("phiy phiz sampx sampy")
# Returns non null on error.
#
def ice_helical_config(str) '{
global HELICAL_PARS[]
local ret
local mnes[]
local mne, num, dev, adr
local cmds[], tt
# Get the list of concerned axes
ret = -1
split(str, mnes)
for(i in mnes) {
# Only IcePAP axis can be used
mne = mnes[i]
num = motor_num(mne)
if(motor_par(num, "device_id") != "icepap") {
_icepap_err
printf("motor not an IcePAP one: \"%s\"\n", mne)
return(ret)
}
# Record axis addressing information
dev = motor_par(num, "address")
adr = motor_par(num, "module")*10 + motor_par(num, "channel")
# Prepare the stop commands, one per system
if(!(dev in cmds)) { cmds[dev] = "STOP" }
cmds[dev] = sprintf("%s %d", cmds[dev], adr)
}
# Record the validated list of mnemonics
HELICAL_PARS["ice_mnes"] = str
# Record the stop commands and the systems list
tt = ""
for(dev in cmds) {
tt = sprintf("%s %s", dev, tt)
HELICAL_PARS[dev] = cmds[dev]
}
HELICAL_PARS["ice_systems"] = icepap_trim(tt)
# Normal end
return(0)
}'
#%UU%(phi_delta_deg)
#%MDESC%
# Configure concerned IcePAP axes for Phi rotation motion of
# the given degrees. The helical parameters of the axes are
# taken from MxCUBE HELICAL_OSCIL_POS[] global resource.
#
def ice_helical_arm(phi_delta) '{
global HELICAL_OSCIL_POS[]
global HELICAL_PARS[]
global HELICAL_OSCIL
local ret
local i, n
local pmux_done[], mnes[]
local mne, num, dev, adr
local dstp, key, key_turn
local ans, tt, anstp, anturn, motpoles
# Ensure that HELICAL_OSCIL_POS[] can be used
ret = -1
if(HELICAL_OSCIL==0) {
_icepap_err
printf("missing helical parameters\n")
return(ret)
}
# Ensure that the axes list has been defined
if(!(whatis("HELICAL_PARS[\"ice_mnes\"]") & 0x84200000)) {
_icepap_err
printf("missing axes list, HINT: call \"ice_helical_config()\"\n")
return(ret)
}
# Record the number of encoder steps that will be received
if((num = motor_num("phi")) == -1) {
_icepap_err
printf("missing \"phi\" motor\n")
return(ret)
}
HELICAL_PARS["phi_dstp"] = fabs(phi_delta)*motor_par(num, "step_size")
# Configure each axis that has to track the incoming encoder signals
n = split(HELICAL_PARS["ice_mnes"], mnes)
for(i=0;i<n;i++) {
# IcePAP axis already checked
mne = mnes[i]
num = motor_num(mne)
# Calculate the increment of this axis in steps
dmm = HELICAL_OSCIL_POS["2"][mne] - HELICAL_OSCIL_POS["1"][mne]
if (dmm == 0) {
_icepap_warn
printf("helical scan does not move motor: \"%s\"\n", mne)
continue
}
dstp = dmm * motor_par(num, "step_size")
key = sprintf("%s_dstp", mne)
HELICAL_PARS[key] = dstp
# Get IcePAP system
dev = motor_par(num, "address")
# Configure the multiplexer.
# A cable with the incoming encoder signals must be plugged
# on MASTER SYNCHRO DB9 connector.
# The signals will be sent to all DRIVERs of the system.
if(!pmux_done[dev]) {
# All signals sent to backplanes and to external outputs
cmd = sprintf("PMUX E0")
ans = _icepap_query(dev, "#", cmd)
if(index(ans,"ERROR")) {
_icepap_err
printf("unable to configure multiplexer\n")
return(ret)
}
# Configuration has to be done once per system
pmux_done[dev] = 1
}
# Get axis address
adr = motor_par(num, "module")*10 + motor_par(num, "channel")
# Get axis motor electrical resolution
ans = _icepap_query(dev, adr, "?cfg anstep")
sscanf(ans, "%s %g", tt, anstp)
key = sprintf("%s_anstep", mne)
HELICAL_PARS[key] = anstp
ans = _icepap_query(dev, adr, "?cfg anturn")
sscanf(ans, "%s %g", tt, anturn)
key = sprintf("%s_anturn", mne)
HELICAL_PARS[key] = anturn
# Calculate the increment of this axis in turns
key_turn = sprintf("%s_dturn", mne)
HELICAL_PARS[key_turn] = (dstp * anturn) / anstp
# Calculate tracking ratio
key_cstp = sprintf("%s_syncres_steps", mne)
key_cturn = sprintf("%s_syncres_turn", mne)
HELICAL_PARS[key_cturn]= 1
HELICAL_PARS[key_cstp] = HELICAL_PARS["phi_dstp"] / HELICAL_PARS[key_turn]
# Check that resolution is not under the motor capabilities
ans = _icepap_query(dev, adr, "?cfg motpoles")
sscanf(ans, "%s %g", tt, motpoles)
if(fabs(HELICAL_PARS[key_cstp]) < (motpoles*16)) {
_icepap_err
printf("too poor resolution of motor \"%s\"\n", mne)
# Do not forget to get out of tracking already configured axes
ice_helical_stop()
return(ret)
}
# Check that resolution has enough significant digits
for(; fabs(HELICAL_PARS[key_cstp])<100000;) {
HELICAL_PARS[key_cstp] *= 10
HELICAL_PARS[key_cturn] *= 10
}
HELICAL_PARS[key_cstp] = icepap_round(HELICAL_PARS[key_cstp])
# Calculation of motor speed
tmp_key = sprintf("%s_velocity", mne)
HELICAL_PARS[tmp_key] = OSCIL_CALCVELOCITY * HELICAL_PARS[sprintf("%s_anstep", mne)]
HELICAL_PARS[tmp_key] /= HELICAL_PARS[sprintf("%s_anturn", mne)]
HELICAL_PARS[tmp_key] *= HELICAL_PARS[sprintf("%s_syncres_turn", mne)]
HELICAL_PARS[tmp_key] /= HELICAL_PARS[sprintf("%s_syncres_steps", mne)]
icepap_cmd(num, "slew_rate", fabs(HELICAL_PARS[tmp_key] * 1.1))
icepap_cmd(num, "acceleration", 100)
# Configure tracking ratio
cmd = sprintf("SYNCRES %d %d", \
HELICAL_PARS[key_cstp], \
HELICAL_PARS[key_cturn])
ans = _icepap_query(dev, "#"adr, cmd)
if(index(ans,"ERROR")) {
_icepap_err
printf("unable to set external sync resolution\n")
return(ret)
}
# Clear sync counter
cmd = sprintf("ENC SYNC 0")
ans = _icepap_query(dev, "#"adr, cmd)
if(index(ans,"ERROR")) {
_icepap_err
printf("unable to clear sync counter\n")
return(ret)
}
# Activate tracking, from here motor can no more move and
# only a STOP can put it back to operation mode.
#cmd = sprintf("TRACK SYNC FULL")
cmd = sprintf("TRACK SYNC")
ans = _icepap_query(dev, "#"adr, cmd)
if(index(ans,"ERROR")) {
_icepap_err
printf("unable to start tracking mode \n")
return(ret)
}
}
# Normal end
p HELICAL_PARS
return(0)
}'
#%UU%()
#%MDESC%
# Put back the concerned IcePAP axes into operation mode
# independent of incoming encoder signals. Should be called at
# the end of the helical scan in any case.
#
def ice_helical_stop() '{
global HELICAL_PARS[]
local i, dev, devs[]
local n, mnes[]
local mne, num
# Ensure that the axes list has been defined
if(!(whatis("HELICAL_PARS[\"ice_systems\"]") & 0x84200000)) {
_icepap_err
printf("missing IcePAPs list, HINT: call \"ice_helical_config()\"\n")
return(ret)
}
# Only a STOP can get axis out of tracking
split(HELICAL_PARS["ice_systems"], devs)
for(i in devs) {
dev = devs[i]
_icepap_wr(sprintf("%s:5000", dev), "", HELICAL_PARS[dev])
}
# Position discrepancies between spec and the
# motor hardware will be silently resolved in favor of the hardware
read_motors(0x06)
# Restore configuration of each axis that has tracked the
# incoming encoder signals
n = split(HELICAL_PARS["ice_mnes"], mnes)
for(i=0;i<n;i++) {
mne = mnes[i]
num = motor_num(mne)
dev = motor_par(num, "address")
icepap_cmd(num, "slew_rate", motor_par(num, "config_slew_rate"))
icepap_cmd(num, "acceleration", motor_par(num, "config_acceleration"))
}
}'
#%MACROS%
#%IMACROS%
#%AUTHOR% MP BLISS (Original 07/2014).
# %BR%$Revision: 1.4 $ / $Date: 2015/05/11 14:36:35 $
#%TOC%
|