#%TITLE% HEXAPODSM.MAC
#%NAME%
# Macros to control SMARACT piezo hexapod from Spec.
#
#%DESCRIPTION%
# This macro set allows you to define macro motors for each
# hexapod virtual axis. Translation axis are in mm and
# rotations in degree
#
# First, declare an hexapod controller in config.
# The ADDR field must be set to the network hostname of the
# SMARACT MCS controller:
#
# MOTORS DEVICE ADDR <>MODE NUM <>TYPE
#%BR% YES hexasm smhexid06 6 Macro Motors
#
# Configure then motors in config referring to that controller.
# Channel assignment will decide on the motor role as follow:
#
#%PRE%
# Channel
# 0 - X
# 1 - Y
# 2 - Z
# 3 - RotX
# 4 - RotY
# 5 - RotZ
#
#%PRE%
#%END%
#
#
#
global HEXASM[]
global HEXASM_CHS[]
HEXASM_CHS[0] = "X"
HEXASM_CHS[1] = "Y"
HEXASM_CHS[2] = "Z"
HEXASM_CHS[3] = "RotX"
HEXASM_CHS[4] = "RotY"
HEXASM_CHS[5] = "RotZ"
#%IU%(mne, type, unit, module, channel)
#%MDESC%
# Macro motor implementation
#
def hexasm_config(mne, type, p1, p2, p3) '{
local dev
local ans
#
# p1==controller index p2==number of motors supported
#
if (type == "ctrl") {
# retrieve the MCS controller hostname
dev = hexasm_ADDR
# the communication port is fixed for MCS controller
if(!index(dev, ":")) {
dev = sprintf("%s:2000", dev)
}
# check access to controller
ans = _hexasm_putget(dev, "%info device")
if(index(ans, ".error.")) {
_hexasm_err
printf("unable to reach \"%s\"\n", dev)
return ".error."
}
printf("Using SMARACT HEXAPOD controller \"%s\"\n", \
substr(dev, 0, index(dev,":")-1))
# valid controller at this point
if(!(whatis("HEXASM") & 0x1000000)) {
list_init HEXASM
}
list_add(HEXASM, hexasm_ADDR)
list_setpar(HEXASM, hexasm_ADDR, "dev", dev)
}
#
# p1==unit p2==module p3==channel
#
if (type == "mot") {
local ch
local idx
# minimum check
ch = p3
if((ch<0) || (ch>6)) {
_hexasm_err
printf("invalid channel, must be <=6\n")
return ".error."
}
idx = sprintf("mne_%s", HEXASM_CHS[ch])
list_setpar(HEXASM, hexasm_ADDR, idx, motor_mne(mne))
}
}'
#%IU%(mne, cmd, p1, p2)
#%MDESC%
# Macro motor implementation
#
def hexasm_cmd(mne, cmd, p1, p2) '{
local ans
local dev
local ch
local pos
local idx
# trust that the configuration has been done by SPEC
dev = list_getpar(HEXASM, hexasm_ADDR, "dev")
#
#
#
if (mne == ".." ) {
#
#
if ( cmd == "preread_all" ) {
# TODO: optimization needed but "preread_all" can not
# be trusted, not always called by SPEC
#return(_hexasm_preread_all(dev))
}
#
#
if ( cmd == "start_all" ) {
local cmd
# gather all target positions
cmd = "mov"
for(ch=0;ch<6;ch++) {
idx = sprintf("tgt_%s", HEXASM_CHS[ch])
pos = list_getpar(HEXASM, hexasm_ADDR, idx)
# current unit is mm for translations and
# degrees for rotations
# TODO: allow user to choose translations unit in config
cmd = sprintf("%s %f%s", cmd, pos, (ch<3)?"m":"")
}
# launch the motion
ans = _hexasm_putget(dev, cmd)
if(index(ans, ".error.")) {
_hexasm_err
printf("unable to launch motion: %s\n", substr(ans, 8))
return(".error.")
}
return
}
#
#
if ( cmd == "prestart_all") {
# ensure to have a target postion for all axes even those
# that will not be moved
for(ch=0;ch<6;ch++) {
# the "preread_all" has be called at least once on reconfig
idx = sprintf("pos_%s", HEXASM_CHS[ch])
pos = list_getpar(HEXASM, hexasm_ADDR, idx)
idx = sprintf("tgt_%s", HEXASM_CHS[ch])
list_setpar(HEXASM, hexasm_ADDR, idx, pos)
}
}
# called on <ctrl-c> but after the abort_one on each concerned motor
if ( cmd == "abort_all" ) {
# ignore errors
_hexasm_putget(dev, "stop")
return
}
return
}
# from here handle single motor call
ch = motor_par(mne, "channel")
#
#
#
if(cmd == "position") {
# TODO: found a way to optimize the number of calls
if(index(_hexasm_preread_all(dev), ".error.")) {
return(".error.")
}
idx = sprintf("pos_%s", HEXASM_CHS[ch])
pos = list_getpar(HEXASM, hexasm_ADDR, idx)
return(sprintf("%.15g", pos))
}
#
#
#
if(cmd == "get_status") {
local ret
# no motion by default
ret = 0
# get move status from controller:
# 0 == stopped
# 1 == holding positions actively, but not moving
# 2 == moving
ans = _hexasm_putget(dev, "mst?")
if(index(ans, ".error.")) {
_hexasm_err
printf("unable to read status: %s\n", substr(ans, 8))
return(ret)
}
# at least one leg is still moving
if(int(ans) == 2) {
ret = 0x2
return(ret)
}
return(ret)
}
#
# p1==abs pos, p2==rel pos, with pos in mm or deg
#
if ( cmd == "start_one") {
idx = sprintf("tgt_%s", HEXASM_CHS[ch])
list_setpar(HEXASM, hexasm_ADDR, idx, p1)
return
}
}'
#%IU%(dev)
#%MDESC%
# Macro motor implementation
#
def _hexasm_preread_all(dev) '{
local ans
local ch
local pos
local idx
local tt[]
ans = _hexasm_putget(dev, "pos?")
if(index(ans, ".error.")) {
_hexasm_err
printf("unable to read positions: %s\n", substr(ans, 8))
return(".error.")
}
if(split(ans, tt) != 6) {
_hexasm_err
printf("unable to parse positions\n")
return(".error.")
}
# store positions for afterward
for(ch=0;ch<6;ch++) {
idx = sprintf("pos_%s", HEXASM_CHS[ch])
sscanf(tt[ch], "%f", pos)
# warning: the translation default unit is meters
# and the macro motors unit is mm
if(ch<3) {
pos = pos * 1000.0
}
list_setpar(HEXASM, hexasm_ADDR, idx, pos)
}
return ""
}'
#%UU% [hexapod-number]
#%MDESC%
# Opens an interactive menu for hexapod control
#
def hexasm'{
local hxn addr
local n menu[] cmds[]
local i ret
n++
menu[n] = "Show hexapod information"
cmds[n] = "hexasmshow"
n++
menu[n] = "Dump hexapod controller internals"
cmds[n] = "hexasmdump"
n++
menu[n] = "Hardware Initialization and Ref. Search"
cmds[n] = "hexasmhwinit"
n++
menu[n] = "Set new pivot point"
cmds[n] = "hexasmpivot"
if($#) {
hxn=$1
} else {
hxn=_hexasmselect()
}
addr = list_item(HEXASM, hxn)
for(;;) {
tty_cntl("cl")
tty_cntl("md")
printf("\n <%s>\n",addr)
printf("\n Commands: \n\n")
tty_cntl("me")
for(i=1;i<=n;i++) {
tty_cntl("md");printf(" %d - ",i);tty_cntl("me")
printf("%s\n",menu[i])
}
tty_cntl("md");printf("\n 0 - ");tty_cntl("me")
printf("Quit\n")
ret = getval("\n\n Command? ", 0)
if(!ret) {
break
}
if((ret<1) || (ret>n)) {
continue
}
printf("\n")
eval(sprintf("%s %d", cmds[ret], hxn))
break
}
}'
#%UU% [hexapod-number]
#%MDESC%
# Same as "hexasm" and just for users used to standard hexapod macros
#
def hexasmmenu 'hexasm'
#%UU% [hexapod-number]
#%MDESC%
# Does a full reset on one hexapod
#
def hexasmshow '{
local dev
local addr
local hxn
local val
local ch
local idx
local w1 w2
local tt[]
if($#) {
hxn=$1
} else {
hxn=_hexasmselect()
}
if((hxn<1) || (hxn>list_n(HEXASM))) {
print "Wrong hexapod selected"
exit
}
addr = list_item(HEXASM, hxn)
dev = list_getpar(HEXASM, addr, "dev")
#
#
#
tty_cntl("md")
printf("\n <%s>\n\n",addr)
tty_cntl("me")
w1 = -20
#
#
#
tty_cntl("md")
printf(" %*s", w1, "Axes referenced: ")
tty_cntl("me")
ans = _hexasm_putget(dev, "ref?")
printf("%s\n", (int(ans) == 1)?"yes":"no")
#
#
#
tty_cntl("md")
printf(" %*s", w1, "Axes velocity: ")
tty_cntl("me")
ans = _hexasm_putget(dev, "vel?")
sscanf(ans, "%f", val)
printf("%.4f (mm/s)\n", val*1000.0)
#
#
#
tty_cntl("md")
printf(" %*s", w1, "Axes acceleration: ")
tty_cntl("me")
ans = _hexasm_putget(dev, "acc?")
sscanf(ans, "%f", val)
printf("%.4f (mm/s2)\n", val*1000.0)
#
#
#
tty_cntl("md")
printf(" %*s", w1, "Pivot point X/Y/Z: ")
tty_cntl("me")
ans = _hexasm_putget(dev, "piv?")
if(split(ans, tt) != 3) {
_hexasm_err
printf("unable to parse pivot positions\n")
return(".error.")
}
for(ch=0;ch<3;ch++) {
printf("%.4f ", tt[ch]*1000.0)
}
printf("(mm)\n")
tty_cntl("md")
printf(" %*s", w1, "Pivot mode: ")
tty_cntl("me")
ans = _hexasm_putget(dev, "pvm?")
printf("%s\n", int(ans)?"fixed":"relative")
#
#
#
w1 = -8
w2 = -8
printf("\n")
tty_cntl("md")
printf(" %*s %*s %10s\n", \
w1, "Axis:", w2, "Motor:", "Position:")
tty_cntl("me")
for(ch=0;ch<6;ch++) {
idx = sprintf("mne_%s", HEXASM_CHS[ch])
mne = list_getpar(HEXASM, addr, idx)
if(mne == -1) {
mne = ""
}
idx = sprintf("pos_%s", HEXASM_CHS[ch])
pos = list_getpar(HEXASM, addr, idx)
printf(" %*s %*s %10.6f (%s)\n", \
w1, HEXASM_CHS[ch], w2, mne, pos, (ch<3)?"mm":"deg")
}
}'
#%UU% [hexapod-number]
#%MDESC%
# Dump hexapod controller internal informations
#
def hexasmdump '{
local dev
local addr
local hxn
local ans
if($#) {
hxn=$1
} else {
hxn=_hexasmselect()
}
if((hxn<1) || (hxn>list_n(HEXASM))) {
print "Wrong hexapod selected"
exit
}
addr = list_item(HEXASM, hxn)
dev = list_getpar(HEXASM, addr, "dev")
#
#
#
tty_cntl("md")
printf("\n <%s>\n\n",addr)
tty_cntl("me")
#
#
#
ans = _hexasm_putget(dev, "%info device")
printf("%s\n\n", ans)
ans = _hexasm_putget(dev, "%info network")
printf("%s\n\n", ans)
ans = _hexasm_putget(dev, "%info status")
printf("%s\n\n", ans)
ans = _hexasm_putget(dev, "%info units")
printf("%s\n\n", ans)
}'
#%UU% [hexapod-number]
#%MDESC%
# Does a full reset on one hexapod
#
def hexasmhwinit '{
local dev
local addr
local hxn
local ans
local yn
if($#) {
hxn=$1
} else {
hxn=_hexasmselect()
}
if((hxn<1) || (hxn>list_n(HEXASM))) {
print "Wrong hexapod selected"
exit
}
addr = list_item(HEXASM, hxn)
dev = list_getpar(HEXASM, addr, "dev")
tty_cntl("md");printf("\n WARNING! WARNING!\n");tty_cntl("me")
printf(" A hard reset on the hexapod will move \n")
printf(" the hexapod to its limit switches.\n")
printf(" !! This may be dangerous !!.\n")
tty_cntl("md");printf(" WARNING! WARNING!\n\n");tty_cntl("me")
yn = yesno("\nDo you want to continue",0)
if(!yn) {
exit
}
printf("Launched....")
# blocking call
ans = _hexasm_putget(dev, "ref")
if(index(ans, ".error.")) {
printf("\n")
_hexasm_err
printf("unable to launch motion: %s\n", substr(ans, 8))
exit
}
# check result
ans = _hexasm_putget(dev, "ref?")
if(index(ans, ".error.")) {
printf("\n")
_hexasm_err
printf("unable to launch motion: %s\n", substr(ans, 8))
exit
}
#
printf("%s\n", (int(ans) == 1)?"ok":"fail")
# force motor positions update silently
read_motors(0x06)
}'
#%UU% [hexapod-number]
#%MDESC%
# Set a new pivot point
#
def hexasmpivot '{
local dev
local addr
local hxn
local cmd
local old[] new[]
local old_fix new_fix
local yn
local ch
if($#) {
hxn=$1
} else {
hxn=_hexasmselect()
}
if((hxn<1) || (hxn>list_n(HEXASM))) {
print "Wrong hexapod selected"
exit
}
addr = list_item(HEXASM, hxn)
dev = list_getpar(HEXASM, addr, "dev")
#
#
#
tty_cntl("md")
printf("\n <%s>\n\n",addr)
tty_cntl("me")
ans = _hexasm_putget(dev, "piv?")
if(split(ans, old) != 3) {
_hexasm_err
printf("unable to parse pivot positions\n")
return(".error.")
}
# position are given in meters
for(ch=0;ch<3;ch++) {
old[ch] *= 1000.0
}
ans = _hexasm_putget(dev, "pvm?")
old_fix = int(ans)
new[0] = getval(" X : ", old[0])
new[1] = getval(" Y : ", old[1])
new[2] = getval(" Z : ", old[2])
new_fix = getval(" fix: ", old_fix)
yn = yesno("\nDo you want to continue",0)
if(!yn) {
exit
}
cmd = "piv"
for(ch=0;ch<3;ch++) {
cmd = sprintf("%s %fm", cmd, new[ch])
}
ans = _hexasm_putget(dev, cmd)
if(index(ans, ".error.")) {
_hexasm_err
printf("unable to set new pivot point: %s\n", substr(ans, 8))
return(".error.")
}
cmd = sprintf("pvm %d", int(new_fix)?1:0)
ans = _hexasm_putget(dev, cmd)
if(index(ans, ".error.")) {
_hexasm_err
printf("unable to set new pivot mode: %s\n", substr(ans, 8))
return(".error.")
}
printf("done\n")
}'
#%IU%
#%MDESC%
# Utility macro to select which hexapod to use if several ones
# have been configured.
#
def _hexasmselect() '{
local hxno hx
hxno = list_n(HEXASM)
if (hxno == 1) {
return(1)
} else if (hxno > 1 ) {
printf("Select hexapod from list\n")
for (hx = 1 ; hx <= hxno ; hx++) {
printf(" %2d - %18s ",hx,(hxname=list_item(HEXASM,hx)))
printf("\n")
}
return(getval("\n Number: ",1))
} else {
printf("No hexapod defined. Sorry\n")
return(-1)
}
}'
#%IU%(dev, cmd)
#%MDESC%
# Returns string ".error.xxxxx." in case of timeout or command failure.
#
def _hexasm_putget(dev, cmd) '{
local ans
local ret
local err
local msg
local ln lc
ans = _hexasm_put(dev, cmd)
if(ans != "") {
return(ans)
}
for(ret="";;) {
# blocking call
ans = sock_get(dev, "\r\n")
# handle timeout
if(ans == "") {
ret = ".error.timeout."
break
}
# handle controller error code
if(sscanf(ans, "\!%d", err) == 1) {
# no error
if(!err) {
return(ret)
}
# decode error code
msg = _hexasm_putget(dev, sprintf("%%code? %d", err))
ret = sprintf(".error.%s.", msg)
break
}
# valid line at this point
ret = sprintf("%s%s", ret, ans)
# handle multiline answer
if(!sock_par(dev, "queue")) {
break
}
}
# remove terminator
for(;;) {
ln = length(ret)
lc = substr(ret, ln, 1)
if((lc == "\r") || (lc == "\n")) {
ret = substr(ret, 0, ln-1)
continue
}
break
}
return(ret)
}'
#%IU%(dev, cmd)
#%MDESC%
# Returns string ".error.xxxxx." in case of timeout or command failure.
#
def _hexasm_put(dev, cmd) '{
local ans
local ret
if(!index(cmd, "\n")) {
cmd=cmd"\n"
}
# just in case
sock_par(dev, "flush")
ret = ""
ans = sock_put(dev, cmd)
if(ans == -1) {
ret = ".error.network."
}
return(ret)
}'
#%IU%(dev, cmd)
#%MDESC%
# Cosmetics macro
#
def _hexasm_err '{
tty_cntl("md")
printf("HEXA SMARTACT ERROR: ")
tty_cntl("me")
}'
#%MACROS%
#%IMACROS%
#%AUTHOR% MP / BLISS / (Original May2017)
#%BR%$Revision: 1.2 $ / $Date: 2017/05/12 10:53:18 $
#%TOC%
|
