#%TITLE% followmot.mac
#%NAME%
#
#%CATEGORY%
#%DESCRIPTION%
# This macro set implement the structure for SPEC motors (followers) which should
# change their position according to the position of a master SPEC motor.
# 3 types of behavior are implemented for a follower and can be changed dynamically.
# 1 - single:
# when the master is moving, position of the follower is calculated
# and set in the A[] SPEC array in order the follower will start its
# movement with the Master
#
# 2 - sync:
# the movement of the follower have to be done in the same time of the
# master movment (synchronized start and stop of the movement).
# This case is mainly used during zapenergy scan in which followers must
# be at their optimum value during all the mOvement of the master motor.
#
# 3 - sbs (Step By Step):
# During the Master movement, the follower will calclculate and move
# according to the changing Master position as fast as possible.
# The reading loop of the master position is not implement in this
# macro set.
# This case is mainly used during zapenergy scan when the speed of
# followers and the master cannot fit.
#
# Case 1 is validated using the "followmot_add" macro
# Case 2 is validated using the "followmot_sync" macro
# Case 3 is validated using the "followmot_sbs" maccro
#
# In each case, specifi macros for each followers should be provided. Needed
# values to calculate parameters and positions of the followers will be set
# in the FOLLOWMOT_PAR[] SPEC associative array. The resulting followers
# positions will be stored in the FOLLOWMOT_LIST manage bi stlist.mac
# macro set
#
# Message, Debug and Error macros are generated using hg.mac macro set
#
#%EXAMPLE%
#%DL%
#%DT%XXXsetup%DD%
#%SETUP%
#%UL%
#%LI%
#%XUL%
need hg
hg_generate("followmot")
##############################################################################
########## ##########
########## MACROS CALLED FROM SPEC ##########
########## ##########
##############################################################################
#%IU%
#%MDESC%
# Definition of global variables
#
def followmot_setup '{
# FOLLOWMOT parameters
global FOLLOWMOT_PAR[]
# FOLLOWMOT_PAR["master_pos"] --> contains the new master position value to
# calculate followers position
#
# FOLLOWMOT_PAR["master_start"] |
# FOLLOWMOT_PAR["master_stop"] |--> information on the master movement to synchronize
# FOLLOWMOT_PAR["master_movtime"] | interrested followers
# FOLLOWMOT_PAR["master_acctime"] |
# List of motors used as followers.
global FOLLOWMOT_LIST[]
list_init FOLLOWMOT_LIST
if($# > 0) {
FOLLOWMOT_PAR["master_mne"] = "$1"
}
}'
# Definition of the macro which will contain all the follower.s
# calculation macros
cdef("users_followmot_calc_pos", "", "")
#%UU% (<mot_mne>, <desc>, <func>)
#%MDESC%
# add a followers.
# <mot_mne> must refers to SPEC configured motor
# <func> must be defined
#
def followmot_add(mot_mne, desc, func) '{
if (motor_num(mot_mne) == -1) {
printf("Motor \"%s\" not configured, exit !!\n", mot_mne)
exit
}
if ((whatis(func) & 2) != 2) {
printf("Macro \"%s\" not defined, exit !!\n", func)
exit
}
if (!list_check(FOLLOWMOT_LIST, mot_mne)) {
list_add(FOLLOWMOT_LIST, mot_mne)
}
list_setpar(FOLLOWMOT_LIST, mot_mne, "desc", desc)
list_setpar(FOLLOWMOT_LIST, mot_mne, "sync", 0)
list_setpar(FOLLOWMOT_LIST, mot_mne, "sbs", 0)
list_setpar(FOLLOWMOT_LIST, mot_mne, "mode", "single")
cdef("users_followmot_calc_pos", sprintf("%s\n", func), mot_mne)
}'
# Definition of the macro which will contain all the follower.s
# macros to synchronize their movement to the master movement
cdef("users_followmot_setparam", "", "")
cdef("users_followmot_moveparam", "", "")
cdef("users_followmot_restoreparam", "", "")
cdef("users_followmot_checkspeed", "", "")
#%UU% (<mot_mne>, <set_param_func>, <restore_param_func>)
#%MDESC%
# Allow follower <mot_mne> to adapt its movement to fit the master movement
# <mot_mne> must refer to a valid follower
# <set_param_func> will set the necessary follower parameters
# <restore_param_func> will restore the follower changed parameters
#
def followmot_sync(mot_mne, set_func, move_func, restore_func, check_func) '{
if (!list_check(FOLLOWMOT_LIST, mot_mne)) {
printf("\"%s\" is not a valid follower, exit !!!\n", mot_mne)
exit
}
if ((whatis(set_func) & 2) != 2) {
printf("Macro \"%s\" not defined, exit !!\n", set_func)
exit
}
if ((whatis(move_func) & 2) != 2) {
printf("Macro \"%s\" not defined, exit !!\n", move_func)
exit
}
if ((whatis(restore_func) & 2) != 2) {
printf("Macro \"%s\" not defined, exit !!\n", restore_func)
exit
}
if ((whatis(check_func) & 2) != 2) {
printf("Macro \"%s\" not defined, exit !!\n", check_func)
exit
}
list_setpar(FOLLOWMOT_LIST, mot_mne, "sync", 1)
cdef("users_followmot_setparam" , sprintf("%s\n", set_func), mot_mne)
cdef("users_followmot_moveparam" , sprintf("%s\n", move_func), mot_mne)
cdef("users_followmot_restoreparam", sprintf("%s\n", restore_func), mot_mne)
cdef("users_followmot_checkspeed" , sprintf("%s\n", check_func), mot_mne)
}'
# Definition of the macro which will contain all the follower's macros to
# move outside of spec in Step-By-Step mode
cdef("users_followmot_sbs_move", "", "")
#%UU% (<mot_mne>, <sbs_func>, <register_func>)
#%MDESC%
# Followers will move outside to the value calculated from
# the master position stored in FOLLOWMOT_PAR["master_pos"].
# A <register_func> may be provided to register in ZAP arrays the position
# of each movement
#
def followmot_sbs(mot_mne, sbs_func) '{
if (!list_check(FOLLOWMOT_LIST, mot_mne)) {
printf("\"%s\" is not a valid follower, exit !!!\n")
exit
}
if ((whatis(sbs_func) & 2) != 2) {
printf("Macro \"%s\" not defined, exit !!\n", set_param_func)
exit
}
list_setpar(FOLLOWMOT_LIST, mot_mne, "sbs", 1)
cdef("users_followmot_sbs_move", sprintf("%s\n", sbs_func), mot_mne)
}'
#%UU%
#%MDESC%
# Display all configured followers
#
def followmot_show '{
local ifmot
printf(" O------------- FOLLOWMOT ----------\n")
printf(" | motor | status | desc.\n")
printf(" O----------------------------------\n")
printf(" | master : %s \n", FOLLOWMOT_PAR["master"])
printf(" ..................................\n")
for (ifmot=1 ; ifmot <= list_n(FOLLOWMOT_LIST) ; ifmot++) {
printf(" |%8s | %10s | %s \n", \
FOLLOWMOT_LIST[ifmot], \
list_getpar(FOLLOWMOT_LIST, ifmot, "active")?" [active] ":"[inactive]", \
list_getpar(FOLLOWMOT_LIST, ifmot, "desc"))
}
printf(" ---------------------------------\n")
}'
##############################################################################
########## ##########
########## MONO MACRO MOTOR ##########
########## ##########
##############################################################################
#%IU% (new_master_pos)
#%MDESC%
# Set A[] array of all the active follower's according to new master position
# This macro is call in the Master Macro motor
#
def followmot_move(new_master_pos)'{
local ifmot fmot_mne fmot_num msg fm_act fm_mode
followmot_master_setpos(new_master_pos)
# Followers movements.
for (ifmot=1 ; ifmot <= list_n(FOLLOWMOT_LIST) ; ifmot++) {
fmot_mne = followmot_getname(ifmot)
fmot_num = motor_num(fmot_mne)
fm_mode = followmot_getmode(fmot_mne)
if (followmot_isactive(fmot_mne) && (fm_mode == "single")) {
A[fmot_num] = followmot_getpos(fmot_mne)
followmot_dbg(sprintf("Should move %s to %g", fmot_mne, A[fmot_num]))
} else {
fm_act = list_getpar(FOLLOWMOT_LIST, ifmot, "active")?"active":"inactive"
msg = sprintf("Follower \"%s\" is %s and mode is %s", fmot_mne, fm_act, fm_mode)
followmot_dbg(msg)
}
}
}'
#%IU% (new_master_pos)
#%MDESC%
# Set the new master position and call all follower's calculation macros.
# For each follower, the parameter "position" in the FOLLOWMOT_LIST will be set
#
def followmot_master_setpos(new_master_pos)'{
FOLLOWMOT_PAR["master_pos"] = new_master_pos
users_followmot_calc_pos
}'
#%IU% (mot_mne)
#%MDESC%
# return the "position" parameter of the FOLLOWMOT_LIST for <mot_mne> follower
# -999999 if the <mot_mne> is not a valid follower
#
def followmot_getpos(mot_mne)'{
if (list_check(FOLLOWMOT_LIST, mot_mne)) {
return(list_getpar(FOLLOWMOT_LIST, mot_mne, "position"))
} else {
printf("\"%s\" is not a valid follower\n", mot_mne)
return(-999999)
}
}'
def followmot_getminspeed() '{
local ifmot fmot_mne fmot_num fm_sync_allowed min_speed
# Followers movements.
for (min_speed=1000000,ifmot=1 ; ifmot <= list_n(FOLLOWMOT_LIST) ; ifmot++) {
fmot_mne = followmot_getname(ifmot)
fmot_num = motor_num(fmot_mne)
fm_sync_allowed = list_getpar(FOLLOWMOT_LIST, fmot_mne, "sync")
if (followmot_isactive(fmot_mne) && fm_sync_allowed) {
fmot_speed = list_getpar(FOLLOWMOT_LIST, fmot_mne, "min_speed")
if (min_speed > fmot_speed) {
min_speed = fmot_speed
}
}
}
return(min_speed)
}'
def followmot_checkspeed(en_from, en_to, en_time) '{
local ifmot fmot_mne fmot_num fm_sync_allowed fm_ok
FOLLOWMOT_PAR["e_from"] = en_from
FOLLOWMOT_PAR["e_to"] = en_to
FOLLOWMOT_PAR["e_time"] = en_time
users_followmot_checkspeed
# Followers movements.
for (fm_ok=1,ifmot=1 ; ifmot <= list_n(FOLLOWMOT_LIST) ; ifmot++) {
fmot_mne = followmot_getname(ifmot)
fm_sync_allowed = list_getpar(FOLLOWMOT_LIST, fmot_mne, "sync")
if (followmot_isactive(fmot_mne) && fm_sync_allowed) {
fmot_ok &= list_getpar(FOLLOWMOT_LIST, fmot_mne, "check_speed")
}
}
return(fm_ok)
}'
##############################################################################
########## ##########
########## FOLLOWMOT TOOLS MACROS ##########
########## ##########
##############################################################################
#%UU% (<mot_mne>)
#%MDESC%
# Remove item <mot_mne> from the followers list
#
def followmot_del(mot_mne) '{
if (list_check(FOLLOWMOT_LIST, mot_mne)) {
cdef("users_followmot_setparam", "", mot_mne, "delete")
cdef("users_followmot_restoreparam", "", mot_mne, "delete")
if (list_getparam(FOLLOWMOT_LIST, mot_mne, "sync")) {
cdef("users_followmot_setparam", "", mot_mne, "delete")
cdef("users_followmot_restoreparam", "", mot_mne, "delete")
}
if (list_getparam(FOLLOWMOT_LIST, mot_mne, "sbs")) {
cdef("users_followmot_sbs_move", "", mot_mne, "delete")
}
list_remove(FOLLOWMOT_LIST, mot_mne)
}
}'
#%IU% (mot_mne)
#%MDESC%
# return the active state of the <mot_mne> follower
#
def followmot_isactive(_mot_mne)'{
if (list_check(FOLLOWMOT_LIST, _mot_mne)) {
# print list_check(FOLLOWMOT_LIST, _mot_mne)
return(list_getpar(FOLLOWMOT_LIST, _mot_mne, "active"))
}
return(0)
}'
#%UU% (<motmne>)
#%MDESC%
# Enable follower <motmne> if configured
#
def followmot_enable(motmne) '{
if (list_check(FOLLOWMOT_LIST, motmne)) {
list_setpar(FOLLOWMOT_LIST, motmne, "active", 1)
}
else{
followmot_err(sprintf("followmot_enable() : %s motor not in followers list.", motmne))
}
}'
#%UU% (<motmne>)
#%MDESC%
# Disable follower <motmne> if configured
#
def followmot_disable(motmne) '{
if (list_check(FOLLOWMOT_LIST, motmne)) {
list_setpar(FOLLOWMOT_LIST, motmne, "active", 0)
}
}'
#%UU% ()
#%MDESC%
# Enable all followers
#
def followmot_enable_all() '{
for (ifmot=1 ; ifmot <= list_n(FOLLOWMOT_LIST) ; ifmot++) {
followmot_enable_all(FOLLOWMOT_LIST[ifmot])
}
}'
#%UU% ()
#%MDESC%
# Disable all followers
#
def followmot_disable_all() '{
local ifmot
for (ifmot=1 ; ifmot <= list_n(FOLLOWMOT_LIST) ; ifmot++) {
followmot_disable(FOLLOWMOT_LIST[ifmot])
}
}'
#%UU% (<motmne>, <follow_mode>)
#%MDESC%
# Set movement mode for followers.
# mode are "single" or "sbs"
# In mode "single", the follower will move to the calculated position one time
# starting with the master motor
# In mode "sbs", during Master movement, follower position wil be constantly recalculted
# and follower will move ecah time to its new position.
#
def followmot_setmode(motmne, follow_mode) '{
if (list_check(FOLLOWMOT_LIST, motmne)) {
list_setpar(FOLLOWMOT_LIST, motmne, "mode", follow_mode)
}
}'
#%UU% (<motmne>)
#%MDESC%
# return movement mode for followers.
# mode are "single" or "sbs"
#
def followmot_getmode(motmne) '{
if (list_check(FOLLOWMOT_LIST, motmne)) {
return(list_getpar(FOLLOWMOT_LIST, motmne, "mode"))
}
}'
#%IU% (mot_mne)
#%MDESC%
# return the list of followers mnemonic
#
def followmot_getdependant() '{
local ifmot fm_list
for (fm_list="",ifmot=1 ; ifmot <= list_n(FOLLOWMOT_LIST) ; ifmot++) {
fmot_mne = followmot_getname(ifmot)
fm_list = sprintf("%s %s ", fm_list, fmot_mne)
}
return(fm_list)
}'
#%IU% (ind)
#%MDESC%
# return the motor name of the follower's at index <ind> in the FOLLOWMOT_LIST list
#
def followmot_getname(ind)'{
return(FOLLOWMOT_LIST[ind])
}'
|
