#%TITLE% HEXAPODE.MAC
#%NAME% HEXAPODE.MAC - Macros to access hexapode device server from spec.
#$Revision: 4.10 $
#%DESCRIPTION%
#%PRE%
# This macro set allows you to define pseudo motors on hexapode.
#
# Since version 4.0 (Feb 2006) this macro file allows also to
# define new style macro motors on an hexapod device.
#
# To define new style macro motors you must define:
#
# an hexapod controller in config:
#
# MOTORS DEVICE ADDR <>MODE NUM <>TYPE
# YES hexa id28/hexapode/01 6 Macro Motors
#
# motors in config referring to that controller. Channel assignment
# will decide on the motor role as follow:
#
# Channel
# 0 - X
# 1 - Y
# 2 - Z
# 3 - RotZ
# 4 - RotY
# 5 - RotX
#
# Caution : take care not to swap RotZ and RotX...
#
# for the time being motors defined on arbitrary translations/rotations
# can only be defined as old-style pseudo motors.
#%PRE%
#%BR%
#$Log: hexapode.mac,v $
#Revision 4.10 2015/12/18 15:21:04 perez
#Improve <ctrl-c> handling
#
#Revision 4.9 2015/09/24 13:17:42 perez
#Workaround for unexpected "start_all" calls
#
#Revision 4.8 2015/02/26 10:28:47 perez
#Add macros to change dynamicaly the Ref Position
#
#Revision 4.7 2014/11/04 13:53:44 perez
#Fix bug of several hexapods in same session + fix bug of version 4.4 on fresh restart
#
#Revision 4.6 2014/09/23 11:24:42 perez
#Add <ctrl-c> handle for macro motors
#
#Revision 4.5 2014/09/22 12:24:23 guilloud
#+ global variable to disable hexapods in a loop using hexa_enable hexa_disable
#can be useful to avoid to slow down scans.
#
#Revision 4.4 2012/12/17 09:35:38 lagier
#in hexa_cmd, "get_status", test HEXAGO_POS global has been
#initialised prior to eventually launch the message "too large
#discrepancy on axis" that prevents this message to systematically
#show up after spec restarts from fresh, and as long as no motion has
#been requested on the hexapode.
#
#Revision 4.3 2010/08/31 12:05:02 perez
#Fix cumulative error
#
#Revision 4.1 2006/09/15 14:00:23 lagier
#bug fix in auto doc syntax
#
#Revision 4.0 2006/08/01 11:04:11 lagier
#New style macro motors support.
#
#Revision 3.9 2005/03/23 10:44:50 rey
#Newmode in hexasim declared now as local variable. Created conflict
#with beamline macros.
#
#Revision 3.8 2003/03/27 10:34:37 rey
#Bug in hexamenu corrected.
#The bug made impossible to select from several hexapodes in
#hexamenu.
#
#Revision 3.7 2002/10/30 07:20:17 sole
#variable hxn renamed to hxn0 in hexamenu to avoid problem when
#calling hexashow from hexamenu
#
#Revision 3.7 2002/10/30 08:20:00 sole
#Problem with hxn variable when calling hexashow from hexamenu
#Revision 3.6 2002/04/10 17:06:26 papillon
#Explicit definition of local variables
#Update hexapode position in HEXA_DEV after mv, hexamove or hexaleg command
#(Done in hexa_movecleanup, hexa_legcleanup)
#Add a hook for user position checking called user_hexa_check
#Add logfile and debug flag that were in ID27 version
#
#%BR%
#
#%EXAMPLE%
#%DL%
#%DT% hexapode %DD%
# Shows complete information about current state and position of the
# hexapode
#%DT% hexaadd id43/hexapode/01 %DD%
# It will declare the hexapode id43/hexapode/01 so that it is known
# by the macros.
#%DT% hexamot hexz id43/hexapode/01 main z %DD%
# Defines motor hexz ( previously defined in config ) to move the
# id43/hexapode/01 in the direction defined as Z
#%DT% hexamenu %DD%
# It provides a number of commands useful when starting up with
# a new hexapode. To be used with care.
#%XDL%
#
#%END%
#%IU%(mne, type, unit, module, channel)
#%MDESC%
# Macro motor implementation
#
def hexa_config(mne, type, unit, module, channel) '{
global HEXALOCKED_POS[]
local hexadev
if (type == "ctrl") {
hexadev = hexa_ADDR
hexainit
if (list_item(HEXA_DEV,hexadev) == -1 )
list_add(HEXA_DEV,hexadev)
print "Using HEXAPODE Macros Motor on",hexa_ADDR,"(ctrl unit",unit ")"
# by default, force position update
HEXALOCKED_POS[hexadev] = 0
}
}'
#%IU%(mne, cmd, p1, p2)
#%MDESC%
# Macro motor implementation
#
def hexa_cmd(mne, cmd, p1, p2) '{
global HEXALOCKED_POS[]
global HEXAGO_POS[]
local hexahw_pos[]
local hexapos
local hexadev
local i
#
#
#
if ( mne == ".." ) {
hexadev = hexa_ADDR
#
#
if ( cmd == "start_all" ) {
# NOTE: MP15Sep24: SPEC built-in can call the controller "start_all"
# entry point even if a motion is already running. Therefore
# ignore "start_all" request in this situation.
state = esrf_io( hexadev,"DevHxpdState")
if(state != 0) {
return
}
for(i=0;i<6;i++) { hexahw_pos[i] = HEXAGO_POS[hexadev][i] }
if(esrf_io( hexadev, "DevHxpdMove", hexahw_pos) == -1) {
printf("Error lauching motion.\n")
return
}
return
}
#
#
if ( cmd == "prestart_all") {
if(!HEXALOCKED_POS[hexadev]) {
if(esrf_io( hexadev,"DevHxpdGetPosition", hexahw_pos) == -1) {
printf("Error reading positions. Giving up.\n")
return
}
for(i=0;i<6;i++) { HEXAGO_POS[hexadev][i] = hexahw_pos[i] }
HEXALOCKED_POS[hexadev] = 1
}
return
}
# called on <ctrl-c> but after the abort_one on each concerned motor
if ( cmd == "abort_all" ) {
# NOTE: MP15Nov17: SPEC built-in can call the controller "abort_all"
# entry point even if no motion is already running. Therefore
# ignore "abort_all" request in this situation.
state = esrf_io( hexadev,"DevHxpdState")
if(state == 0) {
return
}
local hxlegs[]
# legs motion will stop immediately even if the DS is in a middle
# of a "from/to" small motion which means that the DS will let
# the .tmp file causing a no more usable DS
printf("\"%s\": motion abort requested\n", hexadev)
ESRF_ERR = -1
esrf_io( hexadev, "DevHxpdStop")
# NOTE: MP15Dec18: the DevHxpdStop switches the server internal
# state directly to HXPD_FAULT==2 even if the motor is sill moving
# (decelerating). Therefore we can not poll the state to detect
# the end of the motion. Use empiric sleep instead (typical
# deceleration time for an hexapod leg: 150ms)
printf("\"%s\": waiting for the end of the motion...\n", hexadev)
sleep(1)
# wait for the end of motion abort
#for(i=0, state=1;(i<100) && ((state == 1) || (state == 2)) ;i++) {
# sleep(0.02)
# state = esrf_io( hexadev,"DevHxpdState")
#}
# the DS will remove the .tmp file and return to Ready state
# if the leg length uncertainty is removed by a set on leg lengths
printf("\"%s\": synchronizing positions...\n", hexadev)
if ((ret=esrf_io(hexadev,"DevHxpdGetLengths",hxlegs)) == -1) {
printf("Error reading legs lengths. Giving up.\n")
printf("Hint: use \"Full reset\" from \"hexamenu\"\n")
return
}
# remove uncertainty
if ((ret=esrf_io(hexadev,"DevHxpdSetLengths",hxlegs)) == -1) {
printf("Error setting legs lengths. Giving up.\n")
printf("Hint: use \"Full reset\" from \"hexamenu\"\n")
return
}
# force internal synchronization for next motion
HEXALOCKED_POS[hexadev] = 0
# normal end of motion abort
printf("\"%s\": hexapod back to operation\n", hexadev)
return
}
return
}
hexadev = motor_par(mne, "address")
unit = motor_par(mne, "unit")
chan = motor_par(mne, "channel")
#
#
#
if ( cmd == "position" ) {
if(esrf_io(hexadev, "DevHxpdGetPosition", hexahw_pos) == -1) {
printf("Error reading positions. Giving up.\n")
return ".error."
}
if ( chan > 2 ) {
hexapos = rad( hexahw_pos[chan] ) * 1000.0
} else {
hexapos = hexahw_pos[chan]
}
return sprintf("%.15g",hexapos)
}
#
#
#
if ( cmd == "get_status" ) {
local ret
local state
local delta
state = esrf_io( hexadev,"DevHxpdState")
# no motion is going on
if(state == 0) {
# if discrepancy already found, avoid checking again
if(HEXALOCKED_POS[hexadev] != 0) {
# if motion is finished, check if delta on positions are acceptable
if(esrf_io( hexadev, "DevHxpdGetPosition", hexahw_pos) == -1) {
printf("Error reading positions. Giving up.\n")
} else {
# unit is mm
delta = 0.1
if (!(whatis("HEXAGO_POS")&0x8000000)) {
for(i=0;i<6;i++) {
if(fabs(hexahw_pos[i]-HEXAGO_POS[hexadev][i]) > delta) {
printf("WARNING: more than %fmm difference", \
delta)
printf(" on axis %d of %s, syncing\n", \
i, hexadev)
HEXALOCKED_POS[hexadev] = 0
break
}
}
}
}
}
ret = 0
# at least one leg is still moving
} else if ( state == 1 ) {
ret = 0x2
# panic, something went wrong
} else {
ret = 0xc
print "Major error, force position synchronization on",hexadev
HEXALOCKED_POS[hexadev] = 0
}
return ret
}
#
#
#
if ( cmd == "start_one") {
unit = motor_par(mne, "unit")
chan = motor_par(mne, "channel")
if ( chan > 2 ) {
HEXAGO_POS[hexadev][chan] = deg(p1 / 1000.0)
} else {
HEXAGO_POS[hexadev][chan] = p1
}
}
#
#
#
if ( cmd == "set_position") {
print "Set position not available for hexapode. For now this has to be "
print " through device server resources. Sorry"
return
}
}'
#%UU%
#%MDESC%
# Adds a hexapode motor to the SPEC application.
# Type the macro with no parameter to see the full syntax
def hexamot '{
local mnemonic device type param
hexainit
if ($# == 4) {
mnemonic = "$1"
device = "$2"
type = "$3"
param = "$4"
} else {
printf(" Usage: ");
tty_cntl("md");print "hexamot mnemonic hexadev type param";tty_cntl("me")
print
print " Where: "
print " Mnemonic: motor mnemonic as in config "
print " Hexadev: device server name for hexapode"
print " Type: < main | trans | rot >"
print " Param: "
print " /type=main/ < x | y | z | rotx | roty | rotz > "
print " /type=trans/ <trans vector/Ref. system>"
print " example: 1,0,0/LRS"
print " /type=rot/ <origin/Ref. sytem/rot.axis/Ref. system>"
print " example: 0,0,0/LRS/1,0,0/IRS"
print " Posible reference systems are: <LRS|IRS|FRS|MRS>"
exit
}
hexamotadd(device,mnemonic,type,param)
}'
#%UU% device-name
#%MDESC%
# Defines a hexapode to the system. Without motors.
# When using hexamot to define a hexapode motor. The hexapode device
# is automatically defined.
def hexaadd '{
hexainit
if (!$#) {
print "Usage: hexaadd device-name"
exit
}
if (list_item(HEXA_DEV,"$1") == -1 )
list_add(HEXA_DEV,"$1")
}'
#%IU%
#%MDESC%
#
def hexainit '{
global HEXA_DEV HEXA_MOT
global HEXA_ON
global HEXA_REF
global HEXA_PAR[]
# HEXA_PAR["disabled"] if 1 : hexapode commands not called.
if (!(whatis("HEXA_DEV") & 0x1000000) ) {
list_init HEXA_DEV
list_init HEXA_MOT
}
HEXA_REF["LRS"]=0
HEXA_REF["IRS"]=1
HEXA_REF["FRS"]=2
HEXA_REF["MRS"]=3
}'
#%IU%
#%MDESC
#
def hexamotadd(dev,mnemonic,type,param) '{
if (list_item(HEXA_DEV,dev) == -1 )
list_add(HEXA_DEV,dev)
if (list_item(HEXA_MOT,mnemonic) == -1 )
list_add(HEXA_MOT,mnemonic)
list_setpar(HEXA_MOT,mnemonic,"dev",dev)
list_setpar(HEXA_MOT,mnemonic,"type",type)
if ( type == "main" ) {
list_setpar(HEXA_MOT,mnemonic,"param",param)
} else if ( type == "trans" ) {
local pars np
np=split(param,pars,"/")
list_setpar(HEXA_MOT,mnemonic,"axis",pars[0])
list_setpar(HEXA_MOT,mnemonic,"ref",HEXA_REF[pars[1]])
} else if ( type == "rot" ) {
local pars np
np=split(param,pars,"/")
list_setpar(HEXA_MOT,mnemonic,"origin",pars[0])
list_setpar(HEXA_MOT,mnemonic,"refo",HEXA_REF[pars[1]])
list_setpar(HEXA_MOT,mnemonic,"axis",pars[2])
list_setpar(HEXA_MOT,mnemonic,"ref",HEXA_REF[pars[3]])
} else {
print "Hexapode motor type not implemented for the moment.Sorry\n"
return(-1)
}
cdef("user_getpangles",sprintf("hexa_getpos(%s)\n",mnemonic), mnemonic,0x01)
cdef("user_checkall", sprintf("hexa_moveone(%s)\n",mnemonic),mnemonic,0x01)
setup_tail("hexa",mnemonic)
if (whatis("HEXA_ON")&0x8000000) {
hexaon
}
}'
#%IU%
#%MDESC%
#
def hexaunsetup '{
cdef("","",$1,"delete")
}'
#### enable / disable for pseudo motors
def hexa_enable '{
global HEXA_PAR
HEXA_PAR["disabled"] = 0
}'
def hexa_disable '{
global HEXA_PAR
HEXA_PAR["disabled"] = 1
}'
def hexa_is_disabled() '{
return HEXA_PAR["disabled"]
}'
def hexa_is_enabled() '{
return HEXA_PAR["disabled"] ? 0 : 1
}'
#%UU%
#%MDESC%
# Activates all hexapode motors
# This macro use cdef and then cannot be called in a loop...
# use in this case hexa_disable to disable your motors.
def hexaon '{
local hxmot hxmne
HEXA_ON=1
cdef("prompt_mac", "if (!HEXA_PAR[\"disabled\"]){hexa_prompt}\n","_hexa_")
cdef("user_getpangles","if (!HEXA_PAR[\"disabled\"]){hexa_getmain}\n","_hexa_", 0x10)
cdef("user_checkall", "if (!HEXA_PAR[\"disabled\"]){hexa_premove}\n","_hexa_", 0x10)
cdef("user_checkall", "if (!HEXA_PAR[\"disabled\"]){hexa_moveall}\n","_hexa2_",0x20)
cdef("user_motorsrun", "if (!HEXA_PAR[\"disabled\"]){hexa_poll}\n","_hexa_")
for (hxmot=1;hxmot<=list_n(HEXA_MOT);hxmot++) {
hxmne = list_item(HEXA_MOT,hxmot)
cdef("user_getpangles",sprintf("hexa_getpos(%s)\n",hxmne), hxmne,0x01)
cdef("user_checkall", sprintf("hexa_moveone(%s)\n",hxmne),hxmne,0x01)
}
#cdef("user_onsim", "hexa_onsim\n","_hexa_")
#cdef("user_offsim", "hexa_offsim\n","_hexa_")
}'
#%UU%
#%MDESC%
# De-activates all hexapode motors
def hexaoff '{
local hxmot hxmne
HEXA_ON=0
cdef("","","_hexa_","delete")
cdef("","","_hexa2_","delete")
for (hxmot=1;hxmot<=list_n(HEXA_MOT);hxmot++) {
hxmne = list_item(HEXA_MOT,hxmot)
cdef("","",hxmne,"delete")
}
}'
def hexalog '{
global HEXALOGFILE
if (HEXALOGFILE)
close(HEXALOGFILE)
if ($#) {
if ("$1" == "Off" || "$1" == "off") {
HEXALOGFILE=0
} else {
HEXALOGFILE = "$1"
}
} else {
HEXALOGFILE = getval(" Log file for hexapode",HEXALOGFILE)
}
}'
def hexadebug '{
global HEXA_DEBUG
if ($#) {
if ("$1" == "ON" || "$1" == "on" || "$1" == "On") {
HEXA_DEBUG=1
} else {
HEXA_DEBUG=0
}
} else {
HEXA_DEBUG= yesno("Add debug messages", HEXA_DEBUG)
}
}'
#%IU%
#%MDESC%
# To use on/off from blmenu macro. You need to use
# blmenuadd to use it. blmenuadd("Hexapode","Hexapode show","hexabody","_hxb_")
def hexabody(mode) '{
if (mode==1) {
if (HEXA_ON) {
hexaoff
} else {
hexaon
}
}
if (mode==2) {
hexashow
}
return(sprintf("%s",HEXA_ON?"On":"Off"))
}'
#%UU%
#%MDESC%
# Moves hexapode to absolute position
def hexamove '{
local hxdev hxin hxout
local hxang angin hexa_prompt
local ret err
hexa_prompt = 1
if (!$#) {
hxn = hexaselect()
} else {
hxn = $1
if ($# == 7) {
hxin[0] = $2
hxin[1] = $3
hxin[2] = $4
angin[0] = $5
angin[1] = $6
angin[2] = $7
hexa_prompt = 0
} else if ( $# != 1 ) {
printf("Usage: hexamove [ hexa-no [ target pos(x6) ] ]\n")
exit
}
}
hxdev = list_item(HEXA_DEV,hxn)
err=0
if ((ret=esrf_io(hxdev,"DevHxpdGetPosition",hxout)) == -1) {
err=1
}
hxang[0] = rad(hxout[3]) * 1000;
hxang[1] = rad(hxout[4]) * 1000;
hxang[2] = rad(hxout[5]) * 1000;
printf("\nCurrent Position:\n\n")
tty_cntl("md")
printf(" X Y Z Phi/RotZ Th/RotY Psi/RotX\n")
tty_cntl("me")
printf("%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", \
hxout[0],hxout[1],hxout[2], \
hxang[0],hxang[1],hxang[2])
if (err) exit
if (hexa_prompt) {
printf("\nEnter new position:\n")
hxin[0] = getval(" X: ",hxout[0])
hxin[1] = getval(" Y: ",hxout[1])
hxin[2] = getval(" Z: ",hxout[2])
angin[0] = getval(" RotZ: ",hxang[0])
angin[1] = getval(" RotY: ",hxang[1])
angin[2] = getval(" RotX: ",hxang[2])
}
for (ii=0;ii<3;ii++) {
if (!isnum(hxin[ii]) || !isnum(angin[ii])) {
print("Input is not a number\n")
exit
}
}
hxin[3] = deg(angin[0]/1000)
hxin[4] = deg(angin[1]/1000)
hxin[5] = deg(angin[2]/1000)
if (!user_hexa_check(hxdev, hxin)) {
print "Hexapode motion ABORTED."
exit
}
if ((ret=esrf_io(hxdev,"DevHxpdMove",hxin)) == -1) {
err=1
}
cdef("cleanup_once",sprintf("hexa_movecleanup(\"%s\")\n",hxdev),"_hexamove_")
tty_cntl("md")
printf("\n X Y Z Phi/RotZ Th/RotY Psi/RotX\n")
tty_cntl("me")
while(( stat = esrf_io(hxdev,"DevHxpdState")) == 1) {
if ((ret=esrf_io(hxdev,"DevHxpdGetPosition",hxout)) != -1) {
hxang[0] = rad(hxout[3]) * 1000;
hxang[1] = rad(hxout[4]) * 1000;
hxang[2] = rad(hxout[5]) * 1000;
printf("%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\r", \
hxout[0],hxout[1],hxout[2], \
hxang[0],hxang[1],hxang[2])
}
}
hexa_check(hxn,"done")
hexa_movecleanup(hxdev)
}'
#%UU%
#%MDESC%
# Moves actuators by length
def hexaleg '{
local hxn hxdev hxin hxout
local ret err k stat
if (!$#) {
hxn = hexaselect()
} else {
hxn = $1
}
hxdev = list_item(HEXA_DEV,hxn)
err=0
if ((ret=esrf_io(hxdev,"DevHxpdGetLengths",hxout)) == -1) {
err=1
}
printf("\nCurrent Lengths:\n\n")
tty_cntl("md")
printf("Leg1 Leg2 Leg3 Leg4 Leg5 Leg6\n")
tty_cntl("me")
printf("%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", \
hxout[0],hxout[1],hxout[2],hxout[3],hxout[4],hxout[5])
if (err) exit
printf("\nEnter new lengths:\n")
for (k=0;k<6;k++) {
hxin[k] = getval(sprintf(" Leg %d ",k+1),hxout[k])
if (!isnum(hxin[k])) {
print("Input is not a number\n")
exit
}
}
if ((ret=esrf_io(hxdev,"DevHxpdMoveActuators",hxin)) == -1) {
err=1
}
cdef("cleanup_once",sprintf("hexa_legcleanup(\"%s\")\n",hxdev),"_hexa_")
tty_cntl("md")
printf("\nLeg1 Leg2 Leg3 Leg4 Leg5 Leg6\n")
tty_cntl("me")
while(( stat = esrf_io(hxdev,"DevHxpdState")) == 1) {
if ((ret=esrf_io(hxdev,"DevHxpdGetLengths",hxout)) != -1) {
printf("%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\r", \
hxout[0],hxout[1],hxout[2],hxout[3],hxout[4],hxout[5])
}
}
hexa_legcleanup(hxdev)
}'
#%UU%
#%MDESC%
# Enters a loop to prompt for positions to check
# No motion is done with this macro
def hexacalc '{
local hxn hxdev hxno hxname
local hxin hxang argout
local absmin absmax
local lstmin lstmax
local ret err
if (!$#) {
hxn=hexaselect()
} else {
hxn = $1
}
if (hxn < 1 || hxn > list_n(HEXA_DEV) ) {
printf("Wrong hexapode selected\n")
exit
}
hxdev = list_item(HEXA_DEV,hxn)
if ((ret=esrf_io(hxdev,"DevHxpdGetPosition",hxin)) == -1) {
err=1
}
hxang[0] = rad(hxin[3]) * 1000;
hxang[1] = rad(hxin[4]) * 1000;
hxang[2] = rad(hxin[5]) * 1000;
while(1) {
err = 0
lstmax = lstmin = -1
if (err) exit
printf("\nEnter position to calc:\n")
hxin[0] = getval(" X: ",hxin[0])
hxin[1] = getval(" Y: ",hxin[1])
hxin[2] = getval(" Z: ",hxin[2])
hxang[2] = getval(" RotX: ",hxang[2])
hxang[1] = getval(" RotY: ",hxang[1])
hxang[0] = getval(" RotZ: ",hxang[0])
hxin[3] = deg(hxang[0]/1000)
hxin[4] = deg(hxang[1]/1000)
hxin[5] = deg(hxang[2]/1000)
ESRF_ERR=-1
if ((ret=esrf_io(hxdev,"DevHxpdCheckPosition",hxin,argout)) == -1) {
err=1
tty_cntl("md")
print "\n !! Position out of limits !!\n\n"
tty_cntl("me")
}
if ( err != 1 ) {
tty_cntl("md")
printf("Leg1 Leg2 Leg3 ")
printf(" Leg4 Leg5 Leg6\n")
tty_cntl("me")
for (leg=0;leg<6;leg++) {
printf("%6.4f ",argout[leg])
if ( argout[leg] < absmin || absmin < 2) absmin = argout[leg]
if ( argout[leg] > absmax ) absmax = argout[leg]
if ( argout[leg] < lstmin || lstmin < 2) lstmin = argout[leg]
if ( argout[leg] > lstmax ) lstmax = argout[leg]
}
}
printf("\n\n")
tty_cntl("md"); printf("Last minimum : "); tty_cntl("me")
printf("%6.4f ",lstmin)
tty_cntl("md"); printf("Last maximum : "); tty_cntl("me")
printf("%6.4f \n",lstmax)
tty_cntl("md"); printf("Abs. minimum : "); tty_cntl("me")
printf("%6.4f ",absmin)
tty_cntl("md"); printf("Abs. maximum : "); tty_cntl("me")
printf("%6.4f \n",absmax)
}
}'
#%IU%
#%MDESC%
def hexa_movecleanup(hxdev) '{
hexa_cleanup
if ((ret=esrf_io(hxdev,"DevHxpdGetPosition",hxout)) != -1) {
list_setpar(HEXA_DEV,hxn,"x", hxout[0])
list_setpar(HEXA_DEV,hxn,"y", hxout[1])
list_setpar(HEXA_DEV,hxn,"z", hxout[2])
list_setpar(HEXA_DEV,hxn,"phi", hxout[3])
list_setpar(HEXA_DEV,hxn,"theta",hxout[4])
list_setpar(HEXA_DEV,hxn,"psi", hxout[5])
hxang[0] = rad(hxout[3]) * 1000;
hxang[1] = rad(hxout[4]) * 1000;
hxang[2] = rad(hxout[5]) * 1000;
printf("%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", \
hxout[0],hxout[1],hxout[2], \
hxang[0],hxang[1],hxang[2])
}
cdef("", "", "_hexamove_", "delete")
}'
#%IU%
#%MDESC%
def hexa_legcleanup(hxdev) '{
local hxout
hexa_cleanup
if ((ret=esrf_io(hxdev,"DevHxpdGetLengths",hxout)) != -1) {
printf("%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", \
hxout[0],hxout[1],hxout[2],hxout[3],hxout[4],hxout[5])
}
_hexapos(hxdev)
}'
#%UU% [ hexapode-number ]
#%MDESC%
# Alias to hexashow:
# Macro to visualize state/position/lengths of hexapode
def hexapode 'hexashow $*'
#%UU% [ hexapode-number ]
#%MDESC%
# Macro to visualize state/position/lengths of hexapode
def hexashow '{
local hxdev hxn hxname
if (!$#) {
hxn=hexaselect()
} else {
hxn = $1
}
if (hxn < 1 || hxn > list_n(HEXA_DEV) ) {
printf("Wrong hexapode selected\n")
exit
}
_hexadesc(hxn)
_hexalength(hxn)
_hexapos(hxn)
printf(" X = %10.5f (mm) | Leg1: %8.4f - %s\n", \
HEXAPOS[0],HEXALENGTH[0],HEXA_ACTSTATE[0])
printf(" Y = %10.5f (mm) | Leg2: %8.4f - %s\n", \
HEXAPOS[1],HEXALENGTH[1],HEXA_ACTSTATE[1])
printf(" Z = %10.5f (mm) | Leg3: %8.4f - %s\n", \
HEXAPOS[2],HEXALENGTH[2],HEXA_ACTSTATE[2])
printf(" Phi /RotZ = %10.5f (mrad) | Leg4: %8.4f - %s\n", \
rad(HEXAPOS[3])*1000,HEXALENGTH[3],HEXA_ACTSTATE[3])
printf(" Theta/RotY = %10.5f (mrad) | Leg5: %8.4f - %s\n", \
rad(HEXAPOS[4])*1000,HEXALENGTH[4],HEXA_ACTSTATE[4])
printf(" Psi /RotX = %10.5f (mrad) | Leg6: %8.4f - %s\n", \
rad(HEXAPOS[5])*1000,HEXALENGTH[5],HEXA_ACTSTATE[5])
}'
#%UU% [ hexapode-number ]
#%MDESC%
# Shows position of one hexapode
def hexapos '{
local hxn
if ($#) {
hxn=$1
} else {
hxn=hexaselect()
}
if (hxn<1 || hxn > list_n(HEXA_DEV)) {
print "Wrong hexapode selected"
exit
}
_hexapos(hxn)
printf(" X = %10.5f (mm) \n", HEXAPOS[0])
printf(" Y = %10.5f (mm) \n", HEXAPOS[1])
printf(" Z = %10.5f (mm) \n", HEXAPOS[2])
printf(" Phi /RotZ = %10.5f (mrad) \n", rad(HEXAPOS[3])*1000)
printf(" Theta/RotY = %10.5f (mrad) \n", rad(HEXAPOS[4])*1000)
printf(" Psi /RotX = %10.5f (mrad) \n", rad(HEXAPOS[5])*1000)
}'
#%UU% [ hexapode-number ]
#%MDESC%
# Shows general description/state for one hexapode
def hexadesc '{
local hxn
if ($#) {
hxn=$1
} else {
hxn=hexaselect()
}
if (hxn<1 || hxn > list_n(HEXA_DEV)) {
print "Wrong hexapode selected"
exit
}
_hexadesc(hxn)
}'
#%UU% [ hexapode-number ]
#%MDESC%
# Shows leg lengths of one hexapode
def hexalength '{
local hxn
if ($#) {
hxn=$1
} else {
hxn=hexaselect()
}
if (hxn<1 || hxn > list_n(HEXA_DEV)) {
print "Wrong hexapode selected"
exit
}
_hexalength(hxn)
}'
#%IU%
#%MDESC%
#
def _hexadesc(hxn) '{
local err lerror lerrstr descr hexastat hexadev opmode
hexadev = list_item(HEXA_DEV,hxn)
err=0
if ((descr=esrf_io(hexadev,"DevHxpdDescription")) == -1) {
err=1
}
if ((opmode=esrf_io(hexadev,"DevHxpdGetMode")) == -1) {
err=1
}
if ((hexastat=esrf_io(hexadev,"DevHxpdStatus")) == -1) {
err=1
}
lerror=""
if (hexastat == "FAULT") {
lerror=esrf_io(hexadev,"DevHxpdLastError")
lerr=lerror&~((0x3)<<26)&~((0x1)<<18)
if (lerror==-1) err=1
lerrstr=esrf_db("ERROR/3/1",lerr)
}
if (err) {
printf("Error accessing device %s\n", hexadev)
return
}
printf("\n Hexapode: ")
tty_cntl("md");printf(" <%s>\n\n",hexadev);tty_cntl("me")
printf(" Description: %s\n",descr)
printf(" State: ")
tty_cntl("md");printf("%s - %s\n",hexastat,lerrstr);tty_cntl("me")
printf(" Mode: ")
tty_cntl("md");printf("%s\n\n",opmode?"Simulation":"Normal");tty_cntl("me")
}'
#%IU%
#%MDESC%
#
def _hexalength(hxn) '{
global HEXALENGTH HEXA_ACTSTATE
local argout lstate ststr
hexadev = list_item(HEXA_DEV,hxn)
if (esrf_io(hexadev, "DevHxpdActuatorStatus", lstate) != 6) {
printf("Error accessing device %s\n", hexadev)
exit
}
if (esrf_io(hexadev, "DevHxpdGetLengths", HEXALENGTH) != 6) {
printf("Error accessing device %s\n", hexadev)
} else {
for (leg=0;leg<6;leg++) {
HEXA_ACTSTATE[leg]=""
if (lstate[leg] & 0x01 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"READY")
if (lstate[leg] & 0x02 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"MOVING")
if (lstate[leg] & 0x04 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"UPLIM")
if (lstate[leg] & 0x08 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"DOWNLIM")
if (lstate[leg] & 0x10 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"HOME")
if (lstate[leg] & 0x20 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"SEARCH")
if (lstate[leg] & 0x80 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"FAULT")
}
}
}'
#%IU%
#%MDESC%
#
def _hexapos(hxn) '{
global HEXAPOS
local hexadev
if (HEXA_DEBUG) {
print "_hexapos: reload position"
}
hexadev = list_item(HEXA_DEV,hxn)
if (esrf_io(hexadev, "DevHxpdGetPosition", HEXAPOS) != 6) {
printf("Error accessing device %s\n", hexadev)
}
list_setpar(HEXA_DEV,hxn,"x", HEXAPOS[0])
list_setpar(HEXA_DEV,hxn,"y", HEXAPOS[1])
list_setpar(HEXA_DEV,hxn,"z", HEXAPOS[2])
list_setpar(HEXA_DEV,hxn,"phi", HEXAPOS[3])
list_setpar(HEXA_DEV,hxn,"theta",HEXAPOS[4])
list_setpar(HEXA_DEV,hxn,"psi", HEXAPOS[5])
}'
#%UU% [ hexapode-number ]
#%MDESC%
# Set operation mode to NORMAL
def hexanormal '{
local hxn hexadev
if ($#) {
hxn=$1
} else {
hxn=hexaselect()
}
if ( hxn <1 || hxn>list_n(HEXA_DEV)) {
print "Wrong hexapode selected"
exit
}
hexadev = list_item(HEXA_DEV,hxn)
esrf_io(hexadev,"DevHxpdSetMode",0)
}'
def hexa_onsim '{
hxno = list_n(HEXA_DEV)
for (hx = 1 ; hx <= hxno ; hx++) {
hexadev=list_item(HEXA_DEV,hx)
esrf_io(hexadev,"DevHxpdSetMode",1)
}
}'
def hexa_offsim '{
hxno = list_n(HEXA_DEV)
for (hx = 1 ; hx <= hxno ; hx++) {
hexadev=list_item(HEXA_DEV,hx)
esrf_io(hexadev,"DevHxpdSetMode",0)
}
}'
#%UU% [ hexapode-number ]
#%MDESC%
# Set operation mode to SIMULATION
def hexasim '{
local mode newmode hxn
if ($#) {
hxn=$1
} else {
hxn=hexaselect()
}
hexadev = list_item(HEXA_DEV,hxn)
if ( hxn <1 || hxn>list_n(HEXA_DEV)) {
print "Wrong hexapode selected"
exit
}
mode = esrf_io(hexadev,"DevHxpdGetMode")
printf("\nHexapode %s was in ",hexadev)
tty_cntl("md");printf("%s",mode?"SIMULATION":"NORMAL");tty_cntl("me")
newmode = mode?0:1
esrf_io(hexadev,"DevHxpdSetMode",newmode)
printf(" mode. Now is ")
mode = esrf_io(hexadev,"DevHxpdGetMode")
tty_cntl("md");printf("%s.\n",mode?"SIMULATION":"NORMAL");tty_cntl("me")
}'
#%UU% [ hexapode-number ]
#%MDESC%
# Does a soft reset on one hexapode
def hexareset '{
if ($#) {
hxn=$1
} else {
hxn=hexaselect()
}
if ( hxn <1 || hxn>list_n(HEXA_DEV)) {
print "Wrong hexapode selected"
exit
}
hexadev = list_item(HEXA_DEV,hxn)
esrf_io(hexadev,"DevHxpdReset",1)
}'
#%UU% [hexapode-number]
#%MDESC%
# Does a full reset on one hexapode
def hexahard '{
if ($#) {
hxn=$1
} else {
hxn=hexaselect()
}
if ( hxn <1 || hxn>list_n(HEXA_DEV)) {
print "Wrong hexapode selected"
exit
}
hexadev = list_item(HEXA_DEV,hxn)
tty_cntl("md");printf("\n WARNING! WARNING!\n");tty_cntl("me")
printf(" A hard reset on the hexapode will move \n")
printf(" the hexapode to its limit switches.\n")
printf(" !! This may be dangerous !!.\n")
tty_cntl("md");printf("\n WARNING! WARNING!\n");tty_cntl("me")
_yn = yesno("Do you want to continue",0)
if (_yn) {
esrf_io(hexadev,"DevHxpdReset",0)
}
}'
#%IU%
#%MDESC%
# Utility macro for other macros
def hexaselect() '{
hxno = list_n(HEXA_DEV)
if (hxno == 1) {
return(1)
} else if (hxno > 1 ) {
printf("Select hexapode from list\n")
for (hx = 1 ; hx <= hxno ; hx++) {
printf(" %2d - %18s ",hx,(hxname=list_item(HEXA_DEV,hx)))
desc = esrf_io(hxname,"DevHxpdDescription")
if (desc != -1 ) {
tty_cntl("md");printf("%s",desc);tty_cntl("me")
}
printf("\n")
}
return(getval("\n Number: ",1))
} else {
printf("No hexapode defined. Sorry\n")
return(-1)
}
}'
#%IU%
#%MDESC%
def hexa_prompt '{
local hexa
for (hexa=1;hexa<=list_n(HEXA_DEV);hexa++) {
list_setpar(HEXA_DEV,hexa,"notread",0)
}
cdef("","","hexaclean","delete")
}'
#%IU%
#%MDESC%
def hexa_getmain '{
local hexmot motnum type dev hexa hexamot mne param
for (hexa=1;hexa<=list_n(HEXA_DEV);hexa++) {
if (list_getpar(HEXA_DEV,hexa,"notread") == 1) {
continue
} else {
_hexapos(hexa)
}
}
for (hexmot=1;hexmot<=list_n(HEXA_MOT);hexmot++) {
dev = list_getpar(HEXA_MOT,hexmot,"dev")
type = list_getpar(HEXA_MOT,hexmot,"type")
param = list_getpar(HEXA_MOT,hexmot,"param")
if (type == "main") {
mne = list_item(HEXA_MOT,hexmot)
motnum = motor_num(mne)
if (param == "x") {
A[motnum] = user(motnum,list_getpar(HEXA_DEV,dev,"x"))
if (HEXA_DEBUG) { print "hexa_getmain: set", mne, A[motnum] }
}
if (param == "y") {
A[motnum] = user(motnum,list_getpar(HEXA_DEV,dev,"y"))
if (HEXA_DEBUG) { print "hexa_getmain: set", mne, A[motnum] }
}
if (param == "z") {
A[motnum] = user(motnum,list_getpar(HEXA_DEV,dev,"z"))
if (HEXA_DEBUG) { print "hexa_getmain: set", mne, A[motnum] }
}
if (param == "rotx") {
A[motnum] = user(motnum,rad(1000 * list_getpar(HEXA_DEV,dev,"psi")))
if (HEXA_DEBUG) { print "hexa_getmain: set", mne, A[motnum] }
}
if (param == "roty") {
A[motnum] = user(motnum,rad(1000 * list_getpar(HEXA_DEV,dev,"theta")))
if (HEXA_DEBUG) { print "hexa_getmain: set", mne, A[motnum] }
}
if (param == "rotz") {
A[motnum] = user(motnum,rad(1000 * list_getpar(HEXA_DEV,dev,"phi")))
if (HEXA_DEBUG) { print "hexa_getmain: set", mne, A[motnum] }
}
}
}
}'
#%IU%
#%MDESC%
def hexa_premove '{
global HEXA_ABORTED
local hexa
HEXA_ABORTED=0
for (hexa=1;hexa<=list_n(HEXA_DEV);hexa++) {
list_setpar(HEXA_DEV,hexa,"notread",1)
list_setpar(HEXA_DEV,hexa,"activemain","")
list_setpar(HEXA_DEV,hexa,"activeother","")
list_setpar(HEXA_DEV,hexa,"aborted",0)
}
}'
#%IU%
#%MDESC%
def hexa_moveall '{
local mot hexa mainlist otherlist
local motno mne type
for (hexa=1;hexa<=list_n(HEXA_DEV);hexa++) {
if (list_getpar(HEXA_DEV,hexa,"movethis")) {
mainlist =list_getpar(HEXA_DEV,hexa,"activemain")
otherlist=list_getpar(HEXA_DEV,hexa,"activeother")
if ((mainlist != "") && (otherlist != "")) {
printf("Incompatible movements on same hexapode\n")
list_setpar(HEXA_DEV,hexa,"aborted",1)
}
if (otherlist != "" ) {
motno = split(otherlist,motors,",")
if (motno != 1) {
printf("Multiple non-main movements on same hexapode\n")
list_setpar(HEXA_DEV,hexa,"aborted",1)
}
}
HEXA_ABORTED += list_getpar(HEXA_DEV,hexa,"aborted")
if (otherlist != "" ) {
mne = otherlist
type = list_getpar(HEXA_MOT,mne,"type")
if (type=="rot") {
local coor argin axis
orig = list_getpar(HEXA_MOT,mne,"origin")
nco = split(orig,coor,",")
if (nco != 3 ) {
printf("Wrong definition for orig rot motor %s\n",mne)
HEXA_ABORTED=1
}
argin[0] = coor[0]
argin[1] = coor[1]
argin[2] = coor[2]
argin[3] = list_getpar(HEXA_MOT,mne,"refo")
axis = list_getpar(HEXA_MOT,mne,"axis")
nco = split(axis,coor,",")
if (nco != 3 ) {
printf("Wrong definition for axis rot motor %s\n",mne)
HEXA_ABORTED=1
}
argin[4] = coor[0]
argin[5] = coor[1]
argin[6] = coor[2]
argin[7] = list_getpar(HEXA_MOT,mne,"ref")
motnum = motor_num(mne)
if (!HEXA_ABORTED) {
angle = dial(motnum,A[motnum]) - list_getpar(HEXA_MOT,mne,"oldpos")
argin[8] = deg(angle/1000)
dev = list_getpar(HEXA_MOT,mne,"dev")
esrf_io(dev,"DevHxpdRotate",argin)
} else {
A[motnum] = user(motnum,list_getpar(HEXA_MOT,mne,"oldpos"))
}
}
if (type=="trans") {
local coor argin motnum distance argin dev
axis = list_getpar(HEXA_MOT,mne,"axis")
nco = split(axis,coor,",")
if (nco != 3 ) {
printf("Wrong definition for trans motor %s\n",mne)
HEXA_ABORTED=1
}
motnum = motor_num(mne)
if (!HEXA_ABORTED) {
distance = dial(motnum,A[motnum]) - list_getpar(HEXA_MOT,mne,"oldpos")
# Normalize?
argin[0] = coor[0] * distance
argin[1] = coor[1] * distance
argin[2] = coor[2] * distance
argin[3] = list_getpar(HEXA_MOT,mne,"ref")
dev = list_getpar(HEXA_MOT,mne,"dev")
esrf_io(dev,"DevHxpdTranslate",argin)
} else {
A[motnum] = user(motnum,list_getpar(HEXA_MOT,mne,"oldpos"))
}
}
}
if (mainlist != "" ) {
argin[0] = list_getpar(HEXA_DEV,hexa,"x")
argin[1] = list_getpar(HEXA_DEV,hexa,"y")
argin[2] = list_getpar(HEXA_DEV,hexa,"z")
argin[3] = list_getpar(HEXA_DEV,hexa,"phi")
argin[4] = list_getpar(HEXA_DEV,hexa,"theta")
argin[5] = list_getpar(HEXA_DEV,hexa,"psi")
motno = split(mainlist,motors,",")
for (mot=0;mot<motno;mot++) {
mne = motors[mot]
motnum = motor_num(mne)
param = list_getpar(HEXA_MOT,mne,"param")
if (!HEXA_ABORTED) {
if (param == "x") argin[0] = dial(motnum,A[motnum])
if (param == "y") argin[1] = dial(motnum,A[motnum])
if (param == "z") argin[2] = dial(motnum,A[motnum])
if (param == "rotz")
argin[3] = deg(dial(motnum,A[motnum])/1000)
if (param == "roty")
argin[4] = deg(dial(motnum,A[motnum])/1000)
if (param == "rotx")
argin[5] = deg(dial(motnum,A[motnum])/1000)
} else {
A[motnum] = user(motnum,list_getpar(HEXA_MOT,mne,"oldpos"))
}
}
if (!HEXA_ABORTED) {
dev = list_item(HEXA_DEV,hexa)
list_setpar(HEXA_DEV,hexa,"notread",0)
cdef("cleanup_once","hexa_cleanup\n","hexaclean")
hexa_check(dev,"mv",argin)
if (!user_hexa_check(dev, argin)) {
print "Hexapode motion ABORTED."
HEXA_ABORTED= 1
} else {
esrf_io(dev,"DevHxpdMove",argin)
}
}
}
}
}
}'
#%IU% (hxdev, hxpos)
#%MDESC% Hook macro called before hexapode motion in a hexamove or mv command
#%BR%Parameters
#%BR% $1 = device name
#%BR% $2 = position array (x, y, z, rotz, roty, rotx)
#%BR%Should return 0 if motion must be canceled, 1 otherwise
def user_hexa_check(hxdev, hxpos) '{
return 1
}'
#%IU%
#%MDESC%
def hexa_check(hxn,action,argin) '{
local hexout dev hxcheck
global HEXALOG_TO HXERROR
global ESRF_ERR ESRF_ERR_MSG
dev = list_item(HEXA_DEV,hxn)
esrf_io(dev,"DevHxpdGetPosition",hexout)
if (HEXALOGFILE || HEXA_DEBUG) {
dev = list_item(HEXA_DEV,hxn)
esrf_io(dev,"DevHxpdGetPosition",hexout)
if (!HEXA_DEBUG) {
on(HEXALOGFILE);offt
}
if ( action != "done") {
ESRF_ERR=-1
esrf_io(dev,"DevHxpdCheckPosition",argin,hxcheck)
printf("----\n")
p date()
if (ESRF_ERR) {
print "Movement discarded"
print "Reason: ",ESRF_ERR_MSG
HXERROR = 1
}
printf("SPEC: %s Device: %s - Type: %s \n",SPEC,dev,action)
printf("Argin: %s %s %s %s %s %s\n", \
argin[0],argin[1],argin[2],argin[3],argin[4],argin[5])
printf("From: X: %6.6f Y: %6.6f Z: %6.6f \n", \
hexout[0],hexout[1],hexout[2])
printf(" Phi: %6.6f Theta:%6.6f Psi: %6.6f \n", \
hexout[3],hexout[4],hexout[5])
printf("To: X: %6.6f Y: %6.6f Z: %6.6f \n", \
argin[0],argin[1],argin[2])
printf(" Phi: %6.6f Theta:%6.6f Psi: %6.6f \n", \
argin[3],argin[4],argin[5])
for (idx in argin) HEXALOG_TO[idx] = argin[idx]
} else if ( action == "done") {
if (HXERROR) {
hexareset hxn
HXERROR = 0
} else {
printf("Done: X: %6.6f Y: %6.6f Z: %6.6f \n", \
hexout[0],hexout[1],hexout[2])
printf(" Phi: %6.6f Theta:%6.6f Psi: %6.6f \n", \
hexout[3],hexout[4],hexout[5])
for (idx in hexout) {
if (fabs(HEXALOG_TO[idx] - hexout[idx]) > 0.0001) {
printf("HEXA DID NOT REACH POSITION\n")
}
}
}
}
if (!HEXA_DEBUG) { ont;off(HEXALOGFILE) }
}
}'
#%IU%
#%MDESC%
def hexa_poll '{
local hexa state dev
for (hexa=1;hexa<=list_n(HEXA_DEV);hexa++) {
if (list_getpar(HEXA_DEV,hexa,"movethis")) {
dev = list_item(HEXA_DEV,hexa)
state = esrf_io(dev,"DevHxpdState")
if (state == 1) {
return(1)
} else {
hexa_check(hexa,"done")
list_setpar(HEXA_DEV,hexa,"movethis",0)
}
}
}
}'
#%IU%
#%MDESC%
def hexa_getpos(num) '{
local mne
mne = motor_mne(num)
list_setpar(HEXA_MOT,mne,"oldpos",dial(num,A[num]))
if (HEXA_DEBUG) {
print "hexa_getpos: set oldpos", mne, list_getpar(HEXA_MOT,mne,"oldpos")
}
}'
#%IU%
#%MDESC%
def hexa_moveone(num) '{
local mne bla dev type
mne = motor_mne(num)
if (!HEXA_ON) return
if (A[num] != user(num,list_getpar(HEXA_MOT,mne,"oldpos"))) {
if (HEXA_DEBUG) {
print "hexa_moveone:", mne, "will move to", A[num]
}
dev = list_getpar(HEXA_MOT,mne,"dev")
type = list_getpar(HEXA_MOT,mne,"type")
list_setpar(HEXA_DEV,dev,"movethis",1)
_bad_lim=0
_chk_lim num A[num]
if (_bad_lim)
list_setpar(HEXA_DEV,dev,"aborted",1)
if (type=="main") {
bla = list_getpar(HEXA_DEV,dev,"activemain")
if (bla == "") {
bla = mne
} else {
bla = sprintf("%s,%s",bla,mne)
}
list_setpar(HEXA_DEV,dev,"activemain",bla)
} else {
bla=list_getpar(HEXA_DEV,dev,"activeother")
if (bla == "") {
bla = mne
} else {
bla = sprintf("%s,%s",bla,mne)
}
list_setpar(HEXA_DEV,dev,"activeother",bla)
}
}
else {
if (HEXA_DEBUG) {
print "hexa_moveone:", mne, "won\'t move"
}
}
}'
#%IU%
#
def hexa_cleanup '{
hexastop
}'
#%UU%
#%MDESC%
#
def hexastop '{
local hexa dev state
for (hexa=1;hexa<=list_n(HEXA_DEV);hexa++) {
dev = list_item(HEXA_DEV,hexa)
state = esrf_io(dev,"DevHxpdState")
if (state == 1) {
esrf_io(dev,"DevHxpdStop")
}
}
}'
#%UU%
#%MDESC%
# Tool for hexapode calibration
def hexamenu '{
global HEXACMD HEXA_NCMD
local hxn0 hxdev
if (!$#) {
hxn0 = hexaselect()
} else {
hxn0 = $1
}
hxdev = list_item(HEXA_DEV,hxn0)
HEXACMD[1] = "Show Hexapode"
HEXACMD[2] = "STOP"
HEXACMD[3] = "Soft reset ( reset error flag )"
HEXACMD[4] = "Set Reference Position"
HEXACMD[5] = "Move hexapode legs"
HEXACMD[6] = "Move absolute to pos (x/y/z)"
HEXACMD[7] = "Switch Mode (NORMAL/SIMULATION)"
HEXACMD[8] = "Set New Lengths Values"
HEXACMD[9] = "Search Home Switches"
HEXACMD[10] = "Full reset ( move ref + move nominal )"
HEXACMD[11] = "Calc and Check Positions"
HEXA_NCMD = 11
tty_cntl("md")
printf("\n < Hexapode Calibration >\n\n")
tty_cntl("me")
while (1) {
newcmd = hexa_showcmds()
if (newcmd) {
if (substr(HEXACMD[newcmd],0,4) == "Show") { # Show hexapode
hexashow hxn0
}
if (substr(HEXACMD[newcmd],0,4) == "STOP") { # Stop
hexastop
}
if (substr(HEXACMD[newcmd],0,4) == "Soft") { # Soft reset
hexareset
}
if (substr(HEXACMD[newcmd],0,7) == "Set Ref") { # Soft reset
hexasetref(hxdev)
}
if (substr(HEXACMD[newcmd],0,6) == "Move h") { # Move legs
hexaleg hxn0
}
if (substr(HEXACMD[newcmd],0,6) == "Move a") { # Move absol
hexamove hxn0
}
if (substr(HEXACMD[newcmd],0,6) == "Switch") { # Switch mode
hexasim hxn0
}
if (substr(HEXACMD[newcmd],0,7) == "Set New") { # Set new lengths
hexaloadpos(hxdev)
}
if (substr(HEXACMD[newcmd],0,6) == "Search") { # Search home
hexasearch(hxdev)
}
if (substr(HEXACMD[newcmd],0,4) == "Full") { # Full reset
hexahard hxn0
}
if (substr(HEXACMD[newcmd],0,4) == "Calc") { # CheckPos
hexacalc hxn0
}
} else {
break
}
}
}'
#%IU%
#%MDESC%
# This macro does a search new home command.
# It reports at the end the position at which the home lengths have
# been found. These should be noted at written into the hexapode
# resource file.
def hexasearch(hxdev) '{
local w
printf("\n\nSearching home: %s\n",hxdev)
if ( esrf_io(hxdev,"DevHxpdSearchHome") == -1 ) {
printf("Error starting seach \n")
exit
}
inloop=1
looperr=0
cdef("cleanup_once","hexa_cleanup\n","__hexa__")
cdef("cleanup_once",sprintf("hexa_newhomelen(\"%s\")\n",hxdev),"__hexasearch__")
tty_cntl("md")
w = 14
printf("%*s %*s %*s %*s %*s %*s\n",w,"Leg1",w,"Leg2",w,"Leg3",w,"Leg4",w,"Leg5",w,"Leg6")
tty_cntl("me")
while(inloop) {
local lstate
if ((hexastat=esrf_io(hxdev,"DevHxpdStatus")) == -1) {
looperr++
if (looperr > 4 ) {
printf("Error reading status. Giving up.\n")
_yn = yesno("Do you want to stop the search",1)
if (_yn) { hexastop }
inloop=0
}
continue
} else {
looperr = 0
if (hexastat == 0) { good = 1 } # 0=Ready,1=Moving,2=Fault
if (hexastat == 2) { good = 0 } # 0=Ready,1=Moving,2=Fault
}
if (esrf_io(hxdev, "DevHxpdActuatorStatus", lstate) != 6) {
printf("Error accessing device %s\n", hexadev)
} else {
for (leg=0;leg<6;leg++) {
HEXA_ACTSTATE[leg]=""
if (lstate[leg] & 0x01 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"READY")
if (lstate[leg] & 0x02 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"MOVING")
if (lstate[leg] & 0x04 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"UPLIM")
if (lstate[leg] & 0x08 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"DOWNLIM")
if (lstate[leg] & 0x10 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"HOME")
if (lstate[leg] & 0x20 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"SEARCH")
if (lstate[leg] & 0x80 )
HEXA_ACTSTATE[leg] = sprintf("%s %s",HEXA_ACTSTATE[leg],"FAULT")
printf("%*s ",w,HEXA_ACTSTATE[leg])
}
printf("\r")
sleep(0.3)
}
}
if (good) {
printf("\n\nSearch Done.\n")
hexa_newhomelen(hxdev)
} else {
printf("\n\nSearch did not complete.\n")
}
cdef("","","__hexa__","delete")
}'
#%IU%
#%MDESC%
def hexa_newhomelen(hxdev) '{
local newlen
tty_cntl("md")
printf("\nNew Home Lengths\n")
tty_cntl("me")
if (esrf_io(hxdev, "DevHxpdNewHomeLengths", newlen) != 6) {
printf("Error accessing device %s\n", hexadev)
} else {
for (leg=0;leg<6;leg++) {
printf("%s/HomeLength%d: %9.6f\n",hxdev,leg+1,newlen[leg])
}
}
}'
#%IU%
#%MDESC%
def hexa_showcmds() '{
tty_cntl("md")
printf("\n Commands:\n\n")
tty_cntl("me")
for (i = 1;i<=HEXA_NCMD;i++) {
tty_cntl("md");printf(" %d - ",i);tty_cntl("me")
printf("%s\n",HEXACMD[i])
}
tty_cntl("md");printf("\n 0 - ");tty_cntl("me")
printf("Quit\n")
ret = getval("\n\n Command? ",0)
return(ret)
}'
#%IU%
#%MDESC%
def hexasetref(hxdev) '{
local err ii hxin hxout
err=0
if ((ret=esrf_io(hxdev,"DevHxpdGetPosition",hxout)) == -1) {
err=1
}
printf("Old position:\n")
hxin[0] = getval(" X : ",hxout[0])
hxin[1] = getval(" Y : ",hxout[1])
hxin[2] = getval(" Z : ",hxout[2])
hxin[3] = getval(" RotZ : ",hxout[3])
hxin[4] = getval(" RotY : ",hxout[4])
hxin[5] = getval(" RotX : ",hxout[5])
printf("Becomes now position:\n")
hxin[6] = getval(" X : ",hxout[0])
hxin[7] = getval(" Y : ",hxout[1])
hxin[8] = getval(" Z : ",hxout[2])
hxin[9] = getval(" RotZ : ",hxout[3])
hxin[10] = getval(" RotY : ",hxout[4])
hxin[11] = getval(" RotX : ",hxout[5])
for (ii=0;ii<12;ii++) {
if (!isnum(hxin[ii])) {
print("Input is not a number\n")
exit
}
}
if ((ret=esrf_io(hxdev,"DevHxpdSetRefPosition",hxin)) == -1) {
err=1
}
if (!err) {
printf("\n\n If you want to keep this settings you should\n")
printf(" enter the following lines in the resource file.\n")
if ((ret=esrf_io(hxdev,"DevHxpdGetRefPosition",hxout)) == -1 ) {
err=1
}
printf("%s/DefRefPositionX: %f\n",hxdev,hxout[0])
printf("%s/DefRefPositionY: %f\n",hxdev,hxout[1])
printf("%s/DefRefPositionZ: %f\n",hxdev,hxout[2])
printf("%s/DefRefPositionPhi: %f\n",hxdev,hxout[3])
printf("%s/DefRefPositionTheta: %f\n",hxdev,hxout[4])
printf("%s/DefRefPositionPsi: %f\n",hxdev,hxout[5])
}
}'
#%IU%
#%MDESC%
def hexaloadpos(hxdev) '{
local err hxin hxout
err=0
if ((ret=esrf_io(hxdev,"DevHxpdGetLengths",hxout)) == -1) {
err=1
}
printf("\nCurrent Lengths:\n\n")
tty_cntl("md")
printf("Leg1 Leg2 Leg3 Leg4 Leg5 Leg6\n")
tty_cntl("me")
printf("%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", \
hxout[0],hxout[1],hxout[2],hxout[3],hxout[4],hxout[5])
if (err) exit
printf("\nEnter new lengths:\n")
for (k=0;k<6;k++) {
hxin[k] = getval(sprintf(" Leg %d ",k+1),hxout[k])
if (!isnum(hxin[k])) {
print "Input is not a number."
exit
}
}
if ((ret=esrf_io(hxdev,"DevHxpdSetLengths",hxin)) == -1) {
err=1
}
}'
#%IU%
#%MDESC%
#
def hexarandom '{
local hxn
if ($#) {
hxn = $1
iter = $2
} else {
hxn = hexaselect()
iter = getval(" Number of iterations",10)
}
hexadev = list_item(HEXA_DEV,hxn)
mode = esrf_io(hexadev,"DevHxpdGetMode")
if ( mode != 1) {
print "This command only works in SIMULATION mode. Sorry"
exit
}
for(i=0;i<iter;i++) {
local val
for (j=0;j<2;j++) {
val[j] = rand()/20000
}
val[2] = rand()/1000 + 1
for (j=3;j<6;j++) {
val[j] = rand()/100000
}
print
print "hexamove " val[0] " " val[1] " " val[2] " " val[3] " " val[4] " " val[5]
hexamove hxn val[0] val[1] val[2] val[3] val[4] val[5]
sleep(3)
p ESRF_ERR
}
}'
#%IU%
#%MDESC%
#
def isnum(variab) '{
if ((variab+0) == (variab)) {
return(1)
} else {
return(0)
}
}'
#%IU%
#%MDESC%
#
def _hexa_select '
# select hexapod to use
if (!$#) {
hxn=hexaselect()
} else {
hxn = $1
}
if (hxn < 1 || hxn > list_n(HEXA_DEV) ) {
printf("Wrong hexapode selected\n")
exit
}
# get access to the concerned hexapod
hxdev = list_item(HEXA_DEV,hxn)
'
#%UU% [hexapod]
#%MDESC%
# Change the Reference Position using the current hexapod position.
# The hexapod will then rotate arround the Reference System
# at the current position (LRS==IRS).
# Only axes X/Y/Z are concerned.
#
def hexasetref '{
local nrefpos[] refpos[]
local i
local hxn hxdev
# select hexapod to use
_hexa_select
# minimum check
tty_cntl("md");printf("\nWARNING! WARNING!\n");tty_cntl("me")
if(!yesno("Use the current position as new reference positon?",0)) {
exit
}
# update hexapod motor positions
_hexapos(hxn)
# calculate new reference position
# only X/Y/Z concerned
for(i=0;i<12;i++) {
# NOTE: MP15Feb26: warning with rotations, the unit seems to be
# in degrees rather than mrad.
nrefpos[i] = (i<3)?HEXAPOS[i]:0.0
}
# change reference position
esrf_io(hxdev,"DevHxpdSetRefPosition", nrefpos)
# NOTE: MP15Feb24: the reference position will be reseted to
# default one, from resources, in case of:
# DevHxpdSetLengths command which is only sent on <ctrl-c>
# DevHxpdReset command called on hexamenu soft reset
#
# force internal synchronization for next motion
HEXALOCKED_POS[hxdev] = 0
}'
#%UU% [hexapod]
#%MDESC%
# Print Reference Position offset compared to default one
# (defined in the Device Server resources)
#
#
def hexashowref '{
local drefpos[] refpos[]
local i v
local hxn hxdev
# select hexapod to use
_hexa_select
# get from resources default reference position
i = 0
sscanf(esrf_io(hxdev,"DevHxpdGetResource", "DefRefPositionX"), "%f", v)
drefpos[i++] = v
sscanf(esrf_io(hxdev,"DevHxpdGetResource", "DefRefPositionY"), "%f", v)
drefpos[i++] = v
sscanf(esrf_io(hxdev,"DevHxpdGetResource", "DefRefPositionZ"), "%f", v)
drefpos[i++] = v
sscanf(esrf_io(hxdev,"DevHxpdGetResource", "DefRefPositionPhi"), "%f", v)
drefpos[i++] = v
sscanf(esrf_io(hxdev,"DevHxpdGetResource", "DefRefPositionTheta"),"%f", v)
drefpos[i++] = v
sscanf(esrf_io(hxdev,"DevHxpdGetResource", "DefRefPositionPsi"), "%f", v)
drefpos[i++] = v
# get current reference position
esrf_io(hxdev,"DevHxpdGetRefPosition", refpos)
#
printf("Offset from default reference position (resources):\n")
printf(" delta X = %10.5f (mm)\n", refpos[0] - drefpos[0])
printf(" delta Y = %10.5f (mm)\n", refpos[1] - drefpos[1])
printf(" delta Z = %10.5f (mm)\n", refpos[2] - drefpos[2])
#printf(" delta Phi /RotZ = %10.5f (mrad)\n", refpos[3] - drefpos[3])
#printf(" delta Theta/RotY = %10.5f (mrad)\n", refpos[4] - drefpos[4])
#printf(" delta Psi /RotX = %10.5f (mrad)\n", refpos[5] - drefpos[5])
}'
#%MACROS%
#%IMACROS%
#%AUTHOR% V. Rey / BLISS / ESRF 1996-1999
#%TOC%
|
