#%TITLE% ICE_ANCILLARY.MAC
#
#%NAME%
# ICE_ANCILLARY.MAC - Control ancillaries on IcePAP axes.
#
#%OVERVIEW%
# Macros needed for IcePAP controlled motors using ancillaries (for instance
# brakes or air pads). The ancillary control (release/engage command)
# and current ancillary status is achieved with an external WAGO box.
#
# The WAGO box is also used to disable IcePAP motor motion (through its
# HOLD signal input). See Electronic Unit for details.
#
#%DESCRIPTION%
#%DL%
#%DT%Configuring macro motors %DD%
# Each IcePAP motor has first to be configured, see "help local ice" for
# details. The motor can be a real one or a linked one. Then the following
# extra parameters have to be configured.
#
#%DT%Extra motor_par() parameters %DD%
# Several ancillary specific parameters have to be given. Using motor_par()
# or in the config through the "Custom Parameters for Motor" page of each
# motor.
# %DL%
# %DT%motor_par(mot, "anc_cmd_ch")%DD%
# The WAGO logical channel used to release/engage the ancillary.
# %DT%motor_par(mot, "anc_cmd_engage")%DD%
# The WAGO channel write value to engage the ancillary.
# The release value will be the complement.
#
# %DT%motor_par(mot, "anc_sta_ch")%DD%
# The WAGO logical channel used to read the ancillary current status.
# %DT%motor_par(mot, "anc_sta_engaged")%DD%
# The WAGO channel read value when the ancillary is engaged.
# The released value will be the complement.
# %XDL%
#%XDL%
#
#%END%
#
# Mandatory standard IcePAP macros
need ice
#%IU%(num, type, p1, p2, p3)
#%MDESC%
# Called by IcePAP macro motor macros on motor reconfig.
#
def _icepap_ancillary_config(num,type,p1,p2,p3) '{
# p1==controller index p2==number of motors supported
if(type=="ctrl") {
if(p1 == 0) {
_icepap_ancillary_setup
}
}
# p1==unit p2==module p3==channel
if(type=="mot") {
global WAGOKEYS[]
local mne
local cmd_ch cmd_ch_missing
local sta_ch sta_ch_missing
local cmd_engage
local sta_engaged
mne = motor_mne(num)
cmd_ch = motor_par(mne, "anc_cmd_ch")
sta_ch = motor_par(mne, "anc_sta_ch")
# NOTE MP 05Dec2016: motor_par() returns 1 if param not defined.
# TODO: better way to detect not set parameter.
cmd_ch_missing = (cmd_ch == -1)
sta_ch_missing = (sta_ch == -1)
# not ancillary on this IcePAP motor
if(cmd_ch_missing && sta_ch_missing) {
# normal end
return(0)
}
# minimum checks
if(cmd_ch_missing) {
_icepap_err
printf("Missing parameter \"%s\" for motor \"%s\"\n", \
"anc_cmd_ch", mne)
return ".error."
}
# minimum checks
if(sta_ch_missing) {
_icepap_err
printf("Missing parameter \"%s\" for motor \"%s\"\n", \
"anc_sta_ch", mne)
return ".error."
}
# minimum checks
if(!(cmd_ch in WAGOKEYS)) {
_icepap_err
printf("Wrong ancillary WAGO channel \"%s\" for motor \"%s\"\n", \
cmd_ch, mne)
return ".error."
}
# minimum checks
if(!(sta_ch in WAGOKEYS)) {
_icepap_err
printf("Wrong ancillary WAGO channel \"%s\" for motor \"%s\"\n", \
sta_ch, mne)
return ".error."
}
# optionnal active levels
anc_cmd_engage = motor_par(mne, "anc_cmd_engage")
anc_sta_engaged = motor_par(mne, "anc_sta_engaged")
# default value is 0 to engage, like this a broken cable
# will engage the ancillary (useful for brakes)
if(anc_cmd_engage == -1) { anc_cmd_engage = 0 }
if(anc_sta_engaged == -1) { anc_sta_engaged = 0 }
printf("ICEPAP: Configuring ancillary for motor \"%s\"\n", mne)
p "\tanc_cmd_ch :", motor_par(mne, "anc_cmd_ch")
p "\tanc_sta_ch :", motor_par(mne, "anc_sta_ch")
p "\tanc_cmd_engage :", anc_cmd_engage
p "\tanc_sta_engaged:", anc_sta_engaged
_icepap_ancillary_add(num, \
cmd_ch, 1-anc_cmd_engage, anc_cmd_engage, \
sta_ch, 1-anc_sta_engaged, anc_sta_engaged)
}
# normal end
return(0)
}'
#%IU%
#%MDESC%
#
def _icepap_ancillary_setup '{
global ICE_ANC[]
global ICE_ANC_TIMEOUT
list_init ICE_ANC
ICE_ANC_TIMEOUT = 1
}'
#%IU%
#%MDESC%
#
def _icepap_ancillary_add(mne, comm_ch, comm_release, comm_engage, st_ch, st_released, st_engaged) '{
local mot
if (motor_num(mne) == -1) {
_icepap_err
printf("motor \"%s\" not difened, exit !!!\n", mot)
exit
}
mot = motor_mne(mne)
list_add(ICE_ANC, mot)
list_setpar(ICE_ANC, mot, "comm_ch", comm_ch)
list_setpar(ICE_ANC, mot, "comm_release", comm_release)
list_setpar(ICE_ANC, mot, "comm_engage", comm_engage)
list_setpar(ICE_ANC, mot, "st_ch", st_ch)
list_setpar(ICE_ANC, mot, "st_released", st_released)
list_setpar(ICE_ANC, mot, "st_engaged", st_engaged)
cdef("user_icepap_prestart_one", \
"_icepap_ancillary_release(\$1)\n", \
"ICE_ANC", \
0x20)
cdef("user_icepap_postmove_one", \
"_icepap_ancillary_engage(\$1)\n", \
"ICE_ANC", \
0x20)
cdef("cleanup_always", "_icepap_ancillary_engage_all()\n", "ICE_ANC")
cdef("user_scan_tail", "_icepap_ancillary_engage_all()\n", "ICE_ANC")
}'
#%IU%
#%MDESC%
#
def _icepap_ancillary_release(mne, linkedmv) '{
local comm_ch comm_release st_ch st_released st_current
local mot st_time
mot = motor_mne(mne)
icepap__debug sprintf("\"%s\": releasing ancillary...", mot)
if (list_check(ICE_ANC, mot) != 0) {
# poll ancillary to be released
# -----------------------------
st_ch = list_getpar(ICE_ANC, mot, "st_ch")
st_released = list_getpar(ICE_ANC, mot, "st_released")
st_current = wago_readch(st_ch)
if (st_current == st_released) {
icepap__debug sprintf("\"%s\": brakes are %s\n", \
mot, st_current==st_released ? "RELEASED" : "ENGAGED ")
return
}
# release ancillary
# -----------------
comm_ch = list_getpar(ICE_ANC, mot, "comm_ch")
comm_release = list_getpar(ICE_ANC, mot, "comm_release")
wago_writech(comm_ch, comm_release)
icepap__debug sprintf("\"%s\": brakes are %s\n", \
mot, st_current==st_released ? "RELEASED" : "ENGAGED ")
for (st_time=time();st_current != st_released;) {
if ((time()-st_time) > ICE_ANC_TIMEOUT) {
_icepap_err
printf("TIMEOUT on \"%s\" brakes releasing\n", mot)
_icepap_ancillary_engage(mne)
exit
}
sleep(0.1)
st_current = wago_readch(st_ch)
icepap__debug sprintf("\"%s\": brakes are %s\n", \
mot, st_current==st_released ? "RELEASED" : "ENGAGED ")
}
# reset motor closed loop
# ----------------------
if(!linkedmv) {
if (ICEPAP[mot]["linked"] == "YES") {
_linkedreset(mot)
} else {
motor_par(mot, "reset_closed_loop")
}
}
}
}'
#%IU%
#%MDESC%
#
def _icepap_ancillary_engage_all() '{
local mot_anc mot
for (mot_anc=1 ; mot_anc<=list_n(ICE_ANC) ; mot_anc++) {
mot = ICE_ANC[mot_anc]
_icepap_ancillary_engage(motor_num(mot), 1)
}
}'
#%IU%
#%MDESC%
#
def _icepap_ancillary_engage(mne, forced, linkedmv) '{
local comm_ch comm_engage mot
mot = motor_mne(mne)
icepap__debug sprintf("\"%s\": engaging ancillary...", mot)
if ((forced == 0) && (_stype & scanType_ScanActive)) {
return
}
if (list_check(ICE_ANC, mot) != 0) {
# poll ancillary to be engaged
# ----------------------------
st_ch = list_getpar(ICE_ANC, mot, "st_ch")
st_engaged = list_getpar(ICE_ANC, mot, "st_engaged")
st_current = wago_readch(st_ch)
if (st_current == st_engaged) {
icepap__debug sprintf("\"%s\": brakes are %s\n", \
mot, st_current==st_engaged ? "ENGAGED " : "RELEASED")
return
}
# engage ancillary
# ----------------
comm_ch = list_getpar(ICE_ANC, mot, "comm_ch")
comm_engage = list_getpar(ICE_ANC, mot, "comm_engage")
wago_writech(comm_ch, comm_engage)
icepap__debug sprintf("\"%s\": brakes are %s\n", \
mot, st_current==st_engaged ? "ENGAGED " : "RELEASED")
for (;st_current != st_engaged;) {
sleep(0.1)
st_current = wago_readch(st_ch)
icepap__debug sprintf("\"%s\": brakes are %s\n", \
mot, st_current==st_engaged ? "ENGAGED " : "RELEASED")
}
# open the motor closed loop
# ----------------------
if (st_current != st_engaged) {
# do not open the closed loop if the brakes can not be engaged
_icepap_warn
printf("\"%s\": brakes not engaged, closed loop let on\n", mot)
return
}
if(!linkedmv) {
if (ICEPAP[mot]["linked"] == "YES") {
# TODO: change the argin from real axes to virtual axis
#_linkedopenclosedloop(mot)
_linkedopenclosedloop(ICEPAP[mot]["linked_names"])
} else {
motor_par(mot, "closed_loop", 0)
}
}
}
}'
#%MACROS%
#%IMACROS%
#%AUTHOR% GB+MP BLISS (Original 12/16).
# %BR%$Revision: 1.1 $ / $Date: 2018/09/03 12:22:57 $
#%TOC%
|
