#%TITLE% HEXAPODE_TANGO.MAC
#%NAME% HEXAPODE_TANGO.MAC - Macros to access hexapode device server from spec.
#%DESCRIPTION%
#%PRE%
# This macro set allows you to macro motors hexapode that will be driven using the conda tango device server.
#
# To define 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 with old taco server.
#%PRE%
#%BR%
#%EXAMPLE%
#%DL% some other usefull commands
#%DT% hexapode %DD%
# Shows complete information about current state and position of the
# hexapode
#%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
}
}'
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%(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 = tango_get( hexadev,"State")
if(state != 0) {
return
}
for(i=0;i<6;i++) { hexahw_pos[i] = HEXAGO_POS[hexadev][i] }
tango_io( hexadev, "Move", hexahw_pos)
return
}
#
#
if ( cmd == "prestart_all") {
if(!HEXALOCKED_POS[hexadev]) {
# if(esrf_io( hexadev,"DevHxpdGetPosition", hexahw_pos) == -1) {
if(tango_get( hexadev,"Position", hexahw_pos) != 6) {
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 = tango_get( hexadev,"State")
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
tango_io( hexadev, "Stop")
# 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 = tango_get( hexadev,"State")
#}
# 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) {
if ((ret=tango_get(hexadev,"LegLength",hxlegs)) != 6) {
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) {
ret = tango_put(hexadev,"LegLength",hxlegs)
# print "LegLength setting returned ",ret
if (ret != 0) {
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) {
if(tango_get( hexadev,"Position", hexahw_pos) != 6) {
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 = tango_get( hexadev,"State")
# 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) {
if(tango_get( hexadev,"Position", hexahw_pos) != 6) {
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 ) {
} else if ( state == 6 ) {
ret = 0x2
# panic, something went wrong
} else {
print "State is ",state
ret = 0xc
print "Major error, force position synchronization on",hexadev
HEXALOCKED_POS[hexadev] = 0
}
return ret
}
#class DevState(IntEnum):
# ON = 0
# OFF = 1
# CLOSE = 2
# OPEN = 3
# INSERT = 4
# EXTRACT = 5
# MOVING = 6
# STANDBY = 7
# FAULT = 8
# INIT = 9
# RUNNING = 10
# ALARM = 11
# DISABLE = 12
# UNKNOWN = 13
#
#
#
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%
# 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] = "Full reset ( move ref + move nominal )"
HEXACMD[5] = "Switch Mode (NORMAL/SIMULATION)"
HEXACMD[6] = "Calc and Check Positions"
HEXA_NCMD = 6
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,6) == "Switch") { # Switch mode
hexasim hxn0
}
if (substr(HEXACMD[newcmd],0,4) == "Full") { # Full reset
hexahard hxn0
}
if (substr(HEXACMD[newcmd],0,4) == "Calc") { # CheckPos
hexacalc hxn0 1
}
} else {
break
}
}
}'
#%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 = tango_get(hxname,"Description")
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_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)
}'
#%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])
}'
def _hexadesc(hxn) '{
local err lerror lerrstr descr hexastat hexadev opmode
hexadev = list_item(HEXA_DEV,hxn)
err=0
if ((descr=tango_get(hexadev,"Description")) == -1) {
err=1
}
if ((opmode=tango_get(hexadev,"Mode")) == -1) {
err=1
}
if ((hexastat=tango_get(hexadev,"Status")) == -1) {
err=1
}
# TODO check LastError return when status is fault, as well as what Status returns in such case
# lerror=""
# if (hexastat == "FAULT") {
# 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) {
if (tango_get(hexadev, "ActuatorStatus", lstate) != 6) {
printf("Error accessing device %s\n", hexadev)
exit
}
# if (esrf_io(hexadev, "DevHxpdGetLengths", HEXALENGTH) != 6) {
if (tango_get(hexadev, "LegLength", 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 (tango_get(hexadev, "Position", 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%
#%MDESC%
#
def hexastop '{
local hexa dev state
for (hexa=1;hexa<=list_n(HEXA_DEV);hexa++) {
dev = list_item(HEXA_DEV,hexa)
# state = tango_get(dev,"State")
# if (state == 1) {
tango_io(dev,"Stop")
# }
}
}'
#%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)
tango_io(hexadev,"Reset",1)
}'
#%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")
mode = tango_get(hexadev,"Mode")
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)
tango_put(hexadev,"Mode",newmode)
printf(" mode. Now is ")
mode = tango_get(hexadev,"Mode")
tty_cntl("md");printf("%s.\n",mode?"SIMULATION":"NORMAL");tty_cntl("me")
}'
#%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) {
tango_io(hexadev,"Reset",0)
}
}'
#%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=tango_get(hxdev,"Position",hxin)) != 6) {
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
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) {
if ((ret=tango_io(hxdev,"CheckPosition",hxin, argout))!=6) {
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)
if ($2) break
}
}'
|
