#%TITLE% shexamacromot.mac
#%NAME%
#shexamacromot.mac
#%DESCRIPTION%
#%PRE%
# Macro to support Symetrie Hexapod BORA or BREVA.
#%PRE%
#%UL%
# %LI% shexastatus%DD%
# %LI% shexaclearerrors%DD%
# %LI% shexaon%DD%
# %LI% shexaoff%DD%
# %LI% shexapoweron%DD%
# %LI% shexapoweroff%DD%
# %LI% shexaref%DD%
#%XUL%
#%EXAMPLE%
#
#%SETUP%
#The motor configuration uses the standard SPEC config pages. %DD%
#In page 1, controller must be set to %B%MAC_MOT%B%,%DD% page 2 DEVICE to %B%shexa%B%, ADDR to ipaddr:255 or Name:255.%DD%
#After rebooting the controler, SPEC checks if the homes have been made. If not the motors are disabled, and you must use %B%shexaref%B% and
#run reconfig.%DD%
#%PRE%
#
#\t\t%B%channel%B%\t\t\t: channel on the controller 1 to 6
#\t\t%B%Step_per_deg/mm%B%\t: must be used to specify the resolution. The effective resolution is [unit]/[step_per_mm*scale_factor].
#example : the controller unit are mm but you want to have positions in microns with 0.020 micron resolution. So you need a scale factor of 1000 and step per deg of 50
#\t\t%B%Sign%B%\t\t\t: may be negative to change the direction
#\t\t%B%Backlash%B%\t\t\t: must be 0
#\t\t%B%Velocity%B% and %B%acceleration%B% are not used. Default values saved in the controller are used.%DD%
#
#The first axis of each controler has an extended configuration feature via the %B%Generic%B% %B%Parameters%B%.%DD%
#If these parameters are not defined, the defaults values saved in the controller are used.
#%UL%
#%LI%1 : Definition of user references, with a comma separated list for tx,ty,tz,rx,ry,rz in units defined in controler. The angles must be positive values.
#example : 0,0,1,0,0,0 Set origin at 1 unit above hexapod
#%LI%2 : Definition of the object's reference%DD%
#example : 0,0,1,0,0,0
#%LI%3 : Definition of units in controller, comma separated list, for translations [m,mm,micron] and rotations [rad,deg,sec]%DD%
#example : mm,rad
#%LI%4 : Scale factor. The value sent to the controller is devided by the scale factor, readings are multiplied by this factor and assigned in spec A[] array%DD%
#example : 1000 convert rad to mrad
#%XUL%
#The %B%shexastatus%B% gives information on the system, and indicates if the controller is on (motor powered, feedback on) or not, if there is error.%DD%
#Errors are buffered and need to be cleared with the %B%shexaclearerrors%B% macro.%DD%
#For debugging purposes, the internal variable %B%SHEXA_IO_DEBUG%B% can be set to 1 or 2%DD%
#\t1:display io actions%DD%
#\t2:write all io actions on "dlog" file%DD%
#%PRE%
#%END%
global SHEXA_SOCKET[]
global SHEXA[]
global SHEXA_ARR[]
global SHEXA_ON
global SHEXA_DEBUG SHEXA_IO_DEBUG SHEXA_DEBUG_T0
global SHEXA_SOCKET[] SHEXA_MOT[]
global SHEXA_ARR[]
global SHEXA_CMD[]
global SHEXA_DISP[]
global SHEXA_UNIT[]
global SHEXA_NB
global SHEXA_POS[]
SHEXA_DEBUG=0
SHEXA_IO_DEBUG = 0
#error messages returned by ERROR? command
SHEXA_ARR["msgError"][0] = "Unknown error"
SHEXA_ARR["msgError"][1] = "System not initialized"
SHEXA_ARR["msgError"][2] = "No communication with axis controller"
SHEXA_ARR["msgError"][3] = "Leg out of limits"
SHEXA_ARR["msgError"][4] = "Following error"
SHEXA_ARR["msgError"][5] = "Drive fault"
SHEXA_ARR["msgError"][6] = "Kinematic error"
SHEXA_ARR["msgError"][7] = "Emergency stop"
SHEXA_ARR["msgError"][8] = "Reference not initialized"
SHEXA_ARR["msgError"][9] = "Command not valid"
SHEXA_ARR["msgError"][10] = "Command rejected, other command still pending"
SHEXA_ARR["msgError"][11] = "Command rejected, feedback impossible"
SHEXA_ARR["msgError"][12] = "Error while saving configuration file"
SHEXA_ARR["msgError"][13] = "Virtual origin refrence : no file"
SHEXA_ARR["msgError"][14] = "Virtual origin refrence : data corrupted"
SHEXA_ARR["msgError"][15] = "Virtual origin refrence : arguments are not coherent"
# returned by STATE#HEXA? command
SHEXA_ARR["msgStatus"][(2*0)+0] = "No error present"
SHEXA_ARR["msgStatus"][(2*0)+1] = "ERROR PRESENT"
SHEXA_ARR["msgStatus"][(2*1)+0] = "Hexapode moving"
SHEXA_ARR["msgStatus"][(2*1)+1] = "Hexapode in position"
SHEXA_ARR["msgStatus"][(2*2)+0] = "Power not active"
SHEXA_ARR["msgStatus"][(2*2)+1] = "Power active"
SHEXA_ARR["msgStatus"][(2*3)+0] = "POM NOT MADE"
SHEXA_ARR["msgStatus"][(2*3)+1] = "POM made"
SHEXA_ARR["msgStatus"][(2*4)+0] = "Not a software/virtual POM"
SHEXA_ARR["msgStatus"][(2*4)+1] = "Software/Virtual POM"
SHEXA_ARR["msgStatus"][(2*5)+0] = "Not in process of taking POM"
SHEXA_ARR["msgStatus"][(2*5)+1] = "POM GOING ON"
SHEXA_ARR["msgStatus"][(2*6)+0] = "Legs within limits"
SHEXA_ARR["msgStatus"][(2*6)+1] = "LEG(S) OUTSIDE LIMITS"
SHEXA_ARR["msgStatus"][(2*7)+0] = "No positioning error"
SHEXA_ARR["msgStatus"][(2*7)+1] = "Hexapode did not reach final position"
SHEXA_ARR["msgStatus"][(2*8)+0] = "No drive fault"
SHEXA_ARR["msgStatus"][(2*8)+1] = "DRIVE FAULT"
SHEXA_ARR["msgStatus"][(2*9)+0] = "No kinematic error"
SHEXA_ARR["msgStatus"][(2*9)+1] = "KINEMATIC ERROR"
SHEXA_ARR["msgStatus"][(2*10)+0] = "Emergency stop not active"
SHEXA_ARR["msgStatus"][(2*10)+1] = "EMERGENCY STOP ACTIVE"
# display units
SHEXA_DISP["trans"][0]="m"
SHEXA_DISP["trans"][1]="mm"
SHEXA_DISP["trans"][2]="micron"
SHEXA_DISP["trans"]["m"]=1
SHEXA_DISP["trans"]["mm"]=2
SHEXA_DISP["trans"]["micron"]=3
SHEXA_DISP["rot"][0]="rad"
SHEXA_DISP["rot"][1]="deg"
SHEXA_DISP["rot"][2]="sec"
SHEXA_DISP["rot"]["rad"]=1
SHEXA_DISP["rot"]["deg"]=2
SHEXA_DISP["rot"]["sec"]=3
SHEXA_CMD["CFG#DISPLAY"]["narg"]=5
#SHEXA_CMD["CFG#LIMITS"]["narg"]=12
SHEXA_CMD["CFG#REPUTIL"]["narg"]=6
SHEXA_CMD["CFG#REPOBJ"]["narg"]=6
SHEXA_CMD["POS"]["narg"]=6
# Hexapod state
constant SHEXA_ERROR 0x01
constant SHEXA_ON_POSITION 0x02
constant SHEXA_FEEDBACK_ON 0x04
constant MOT_MOVING 0x02
constant MOT_LIMP 0x04
constant MOT_LIMM 0x08
constant MOT_EMERGENCY 0X10
constant MOT_FAULT 0X20
#%IU%
#%MDESC%
def _shexa_config_error(mes) '{
printf("%c[%dm",27,31)
printf("\t SHEXA Configuration error : %s\n",mes)
printf("%c[0m",27)
}'
#%IU%
#%MDESC%
#Writes all io action on "dlog" file
def _shexa_io_dbgon '{
open("dlog")
SHEXA_DEBUG_T0=time()
SHEXA_IO_DEBUG=2
}'
#%IU%
#%MDESC%
#Turn off io debuging
def _shexa_io_dbgoff '{
close("dlog")
SHEXA_IO_DEBUG=0
}'
#%IU% [unit] [cmd] [] or [1]
#%MDESC%
#io function
def _shexa_io(unit,cmd,ans) '{
local _ans
if ( SHEXA_ARR[unit]["connected"]!=0 ) {
sock_par(SHEXA_SOCKET[unit], "flush")
sock_put(SHEXA_SOCKET[unit],sprintf("%s%s",cmd,SHEXA_ARR[unit]["term"]))
if( SHEXA_IO_DEBUG==1) { printf("_shexa_io : %s (w) %s\n", SHEXA_SOCKET[unit],cmd) }
if (SHEXA_IO_DEBUG==2) { fprintf("dlog","_shexa_io : %.5f %s (w) %s\n", time()-SHEXA_DEBUG_T0,SHEXA_SOCKET[unit],cmd) }
if ( ans ) {
_ans=sock_get(SHEXA_SOCKET[unit],SHEXA_ARR[unit]["term"])
if( SHEXA_IO_DEBUG==1) { printf(" : %s (r) %s\n", SHEXA_SOCKET[unit],_ans) }
if( SHEXA_IO_DEBUG==2) {fprintf("dlog"," : %.5f %s (r) %s\n",time()-SHEXA_DEBUG_T0, SHEXA_SOCKET[unit],_ans) }
SHEXA_ARR[unit]["connected"]= length(_ans)
return(_ans)
}
} else {
return("")
}
}'
#%IU%
#%MDESC%
def _shexa_getpar(pos,unit,par)'{
local i ans n cmd msg l
l = length(par)+1
n = SHEXA_CMD[par]["narg"]
if ( n==0 ) return(0)
ans = _shexa_io(unit,sprintf("%s?",par),1)
if(length(ans) < l){
print "shexa: Error reading parameters"
return
}
ans = substr(ans,l)
i = split(ans, pos, ",")
if (i !=n ){
print "shexa: Error reading parameter. Incorrect number of data read"
return
}
#remove CR and LF from the last word
pos[n-1] = substr(pos[n-1],0, length(pos[n-1])-2)
}'
#%IU%
#%MDESC%
def _shexa_getstatus_HEXA(unit) '{
local ans
local cmd, lcmd
cmd="STATE#HEXA"
lcmd = length(cmd)
ans= _shexa_io(unit,sprintf("%s?",cmd),1)
if (substr(ans, 0, lcmd) != cmd) {
printf("SHEXA ERROR >> Cannot get status\n")
return (-1)
}
ans= sprintf("0x%s", substr(ans, lcmd+1, 4))
return 1*ans
}'
#%IU%
#%MDESC%
def _shexa_getstatus(unit) '{
local ans cmd,lcmd,_st,_v
# first check if hexapod is moving
cmd="MOV"
ans = _shexa_io(unit,sprintf("%s?",cmd),1)
# if ( index(ans,"1")!=0) { return (MOT_MOVING) }
# read status for more info
_v=_shexa_getstatus_HEXA(unit)
if( SHEXA_DEBUG) { printf("_shexa_getstatus : unit=%d (%s) STATE#HEXA?=0x0%d MOV?=%s\n",unit,SHEXA_SOCKET[unit],_v,ans) }
if ( (_v & 0x01) == 0x01 ) {
if ( (_v & 0x40) == 0x40 ) { return ( MOT_LIMP | MOT_LIMM) }
if ( (_v & 0x400) == 0x400 ) { return ( MOT_EMERGENCY) }
return ( MOT_FAULT )
} else {
# the answer to MOV? just after a MOV command is not MOV1. So we need to wait command_time seconds before testing MOV? answer
# this bug should be corrected in next Symetrie firmware (>3.5.110824)
if ( ((time()-SHEXA_ARR[unit]["command_time"])<2) && (index(ans,"0")!=0) ) {
return (MOT_MOVING)
}
if ( (_v & 0x02) == 0x02 ) { return (0) }
else {
if ( index(ans,"1")!=0 ) { SHEXA_ARR[unit]["command_time"]=0;return (MOT_MOVING) }
return (0)
}
}
}'
def _shexaerror(unit) '{
local ans, value, len,_i
ans = _shexa_io(unit, "ERROR?",1)
len = length(ans)
if (substr(ans, 0, 5) != "ERROR") {
return("SHEXA ERROR >> Cannot get error flag\n")
}
else if (sscanf(substr(ans, 6, len-6-1), "%x", value)==1) {
if (value==0) {
return("SHEXA >> Congrats! No error to be shown.")
} else {
for (_i=0;_i<16;_i++) {
if ( (value & ( 1<<_i))==(1<<_i)) {
return( sprintf("SHEXA ERROR >> %s\n", SHEXA_ARR["msgError"][_i]))
}
}
}
}
}'
########################## MACRO MOTOR CODE ######################################
#%IU%
#%MDESC%
# misc_par_1 definition of user reference x,y,z,rx,ry,rz%DD%
# misc_par_2 definition of object reference x,y,z,rx,ry,rz%DD%
# misc_par_3 units in format a,b where a in [m,mm,micron] b in [rad,deg,sec]%DD%
# misc_par_4 scale factor%DD%
def shexa_config(motormne, type, unit, module, chan) '{
local _s _r _n _a _b _c0 _c1 _term _i _j _cmd _mes _v
if( SHEXA_DEBUG) { printf("shexa_config : motormne=%s type=%s unit=%d channel=%d\n",motormne,type,unit,chan) }
if (motormne=="..") {
_term[0]="\r";_term[1]="\r\n"
unglobal SHEXA_SOCKET SHEXA_MOT SHEXA_POS SHEXA_UNIT SHEXA_NB
global SHEXA_SOCKET[] SHEXA_MOT[] SHEXA_POS[] SHEXA_UNIT[] SHEXA_NB
# try to find which controller is connected and display controller ID
_s=sprintf("%s",shexa_ADDR)
for (_i=0;_i<2;_i++) {
sock_par(_s,"timeout",0.5)
sock_par(_s,"flush")
sock_put(_s,sprintf("HELP?%s",_term[_i]))
#sleep(0.5)
_r=sock_get(_s)
_n=split(_r,_a,"Hexapod ID:")
if (_n>=2) {
_n=split(_a[1],_b,"\r")
printf("\n");
printf("%c[%dm",27,34)
printf("\tUsing Hexapod ID :%s\n",_b[0])
printf("\t%s\n\n",_b[1])
printf("%c[0m",27)
SHEXA_UNIT[unit] = SHEXA_NB
SHEXA_NB++
SHEXA_SOCKET[SHEXA_UNIT[unit]] = shexa_ADDR
SHEXA_ARR[SHEXA_UNIT[unit]]["term"] = _term[_i]
sock_par( SHEXA_SOCKET[SHEXA_UNIT[unit]],"timeout",1)
SHEXA_ARR[SHEXA_UNIT[unit]]["init"]=0
return("")
} else {
sock_put(_s,"\n")
# delay needed otherwise the incomplete command "HELP\r"
# is not answered fast enough when the "\n" is sent
# and the flush does not flush anything, resulting a help answer
# still in the socket buffer
sleep(0.3)
}
}
SHEXA_SOCKET[SHEXA_UNIT[unit]]=""
return(".error.")
} else {
local __unit
__unit = SHEXA_UNIT[unit]
SHEXA_MOT[motormne]["shexa"]=__unit
SHEXA_MOT[motormne]["chan"]=chan+1
SHEXA_MOT[motormne]["init"]=1
SHEXA_ARR[__unit]["connected"]=1
if ( SHEXA_ARR[__unit]["init"]==0) { # first channel is used to configure global parameters
SHEXA_ARR[__unit]["init"]=1
if ( ( _shexa_getstatus_HEXA(__unit)&& 0x04) == 0 ) {
_shexa_config_error(" You need to make reference (shexaref) before using hexapod, and reconfig after ")
SHEXA_ARR[__unit]["init"]=-1 # means error so the other channel will be disabled
return(".error.")
}
# change rep util and rep according to misc_par1 and misc_par2
#printf("Hexapod init \n");
_c0=sscanf(motor_par(motormne,"misc_par_2"),"%f,%f,%f,%f,%f,%f",_v[0],_v[1],_v[2],_v[3],_v[4],_v[5])
_c1=sscanf(motor_par(motormne,"misc_par_1"),"%f,%f,%f,%f,%f,%f",_v[0],_v[1],_v[2],_v[3],_v[4],_v[5])
if ( _c0==6 && _c1==6) {
_shexa_io(__unit,sprintf("CFG#REPOBJ%s",motor_par(motormne,"misc_par_2")))
_shexa_io(__unit,sprintf("CFG#REPUTIL%s",motor_par(motormne,"misc_par_1")))
} else {
if (_c0!=0) { _shexa_config_error(sprintf(" Warning : Wrong parameter number in misc_par_2 for %s",motor_mne(motormne))) }
if (_c0!=0) { _shexa_config_error(sprintf(" Warning : Wrong parameter number in misc_par_1 for %s",motor_mne(motormne))) }
}
# configuration of unit in controller
if ( split(motor_par(motormne,"misc_par_3"),_s,",")!=2) {
_shexa_config_error("Bad unit definition must be [m,mm,micron],[rad,deg,sec]")
return(".error.")
} else {
_i= SHEXA_DISP["trans"][_s[0]]
_j= SHEXA_DISP["rot"][_s[1]]
if ( _i==0 || _j==0 ) {
_shexa_config_error(sprintf(" bad unit definition : %s",motor_par(motormne,"misc_par_3")))
return(".error.")
}
_shexa_io(__unit,sprintf("CFG#DISPLAY%d,%d,%%+.012f,%%+.012f,0",_i-1,_j-1))
}
# clear errors
_shexa_io(__unit,"ERRORCLEAR")
# use absolute move in object referentiel
_shexa_io(__unit,"CFG#MOVE0")
}
# configuration of the scale factor
if ( motor_par(motormne,"misc_par_4")==0 ) {
SHEXA_MOT[motormne]["scale"]=1
} else {
SHEXA_MOT[motormne]["scale"]=motor_par(motormne,"misc_par_4")
}
if ( SHEXA_ARR[__unit]["init"]==-1) {
return(".error.")
}
return("")
}
}'
#%IU%
#%MDESC%
def shexa_cmd(mne, key, p1, p2) '{
if( SHEXA_DEBUG) { printf("shexa_cmd : mne=%s key=%s p1=%.4f p2=%.4f\n",mne,key,p1,p2) }
if (key=="preread_all") {
local _unit
_unit = SHEXA_UNIT[p1]
SHEXA_ARR[_unit]["need_read"]=1
}
if (key=="preread_one") {
local _unit
_unit = SHEXA_UNIT[p1]
SHEXA_ARR[_unit]["need_read"]=1
}
if (key=="position") {
local _u _c pos[] _unit
_unit=SHEXA_UNIT[motor_par(mne,"unit")]
if ( SHEXA_ARR[_unit]["need_read"]==1) {
_shexa_getpar(pos,_unit,"POS")
for (_i=0;_i<6;_i++) { SHEXA_POS[_unit][_i]=pos[_i] }
SHEXA_ARR[_unit]["need_to_read"]=0
}
_u=SHEXA_MOT[mne]["shexa"]
_c=SHEXA_MOT[mne]["chan"]-1
#printf("\t %d %d %.4f\n", _u,_c, SHEXA_POS[_u][_c])
return( SHEXA_POS[_u][_c]*SHEXA_MOT[mne]["scale"] )
}
if (key == "get_status") {
local _u
_u=SHEXA_MOT[mne]["shexa"]
return (_shexa_getstatus(_u))
}
if (key == "start_one") {
local _u _c
_u=SHEXA_MOT[mne]["shexa"]
_c=SHEXA_MOT[mne]["chan"]-1
SHEXA_POS[_u][_c]=p1/SHEXA_MOT[mne]["scale"]
return
}
if (key == "start_all") {
local _i _cmd _unit
_unit = SHEXA_UNIT[p1]
_cmd = sprintf("MOV%.12f",SHEXA_POS[_unit][0] )
for (_i=1;_i<6;_i++){
_cmd=sprintf("%s,%.12f",_cmd ,SHEXA_POS[_unit][_i])
}
if( SHEXA_DEBUG) { printf("shexa_cmd : (%s) _cmd=%s\n",SHEXA_SOCKET[_unit],_cmd) }
_shexa_io(_unit, _cmd)
SHEXA_ARR[_unit]["command_time"]=time()
return
}
if (key == "set_position") {
}
if (key == "diff_position") {
}
if (key == "abort_all") {
local _unit
_unit = SHEXA_UNIT[p1]
if( SHEXA_DEBUG) { printf("shexa_cmd : (%s) abort\n",SHEXA_SOCKET[_unit]) }
_shexa_io(_unit,"STOP")
return("")
}
if (key == "search") {
local _u _c
_u=SHEXA_MOT[mne]["shexa"]
if( SHEXA_DEBUG) { printf("shexa_cmd : (%s) home\n",SHEXA_SOCKET[_u]) }
_shexa_io(_u,"HM#STD")
SHEXA_ARR[_u]["command_time"]=time()
}
}'
#%IU%
#%MDESC%
def shexa_par (motormne,key,type,p1,p2) '{
if ( type == "set" ) {
SHEXA_PAR[motormne][key]=p1
}
if ( type == "get" ){
return(SHEXA_PAR[motormne][key])
}
}'
####################### END CODE for MACRO MOTORS ########################
#%UU% [unit]
#%MDESC%
# Make reference on hexapod
def shexaref '{
local _u _st _f m
if ( $#==0 ) {
printf("Usage : shexaref [unit]\n")
print "Check your unit number with \"shexastatus\""
return
}
_u=$1
_shexa_io(_u,"HM#STD")
while (_shexa_getstatus_HEXA(_u)!=0x0C) {
printf(" Homing in progress on %s : %s\r",SHEXA_SOCKET[_u], substr("-\\|/", (m=++m%4)+1, 1))
sleep(0.1)
}
while (_shexa_getstatus_HEXA(_u)!=0x0E) {
printf(" Homing in progress on %s : %s\r",SHEXA_SOCKET[_u], substr("-\\|/", (m=++m%4)+1, 1))
sleep(0.1)
}
}'
#%UU% [unit]
#%MDESC%
#Turn Hexapode power on
def shexapoweron '{
local _list[] _u _a
if ( $# == 0 ) {
_list=SHEXA_SOCKET
} else {
_list[$1]=" "
}
for ( _u in _list ) {
_shexa_io(_u,"MOTORON")
SHEXA_ARR[_u]["disable"]=0
for ( _i in SHEXA_MOT) {
split(_i,_a,"\034")
if ( SHEXA_MOT[_a[0]]["shexa"]==_u) {
motor_par(_a[0],"disable",0)
}
}
}
}'
#%UU% [unit]
#%MDESC%
#Turn Hexapode on
def shexaon '{
local _list[] _u _a
if ( $# == 0 ) {
_list=SHEXA_SOCKET
} else {
_list[$1]=" "
}
for ( _u in _list ) {
SHEXA_ARR[_u]["disable"]=0
}
}'
#%UU% [unit]
#%MDESC%
#Turn Hexapode power off.
def shexapoweroff '{
local _list _u _a
if ( $# == 0 ) {
_list=SHEXA_SOCKET
} else {
_list[$1]=" "
}
for ( _u in _list ) {
_shexa_io(_u,"MOTOROFF")
SHEXA_ARR[_u]["disable"]=1
for ( _i in SHEXA_MOT) {
split(_i,_a,"\034")
if ( SHEXA_MOT[_a[0]]["shexa"]==_u) {
motor_par(_a[0],"disable",1)
}
}
}
}'
#%UU% [unit]
#%MDESC%
#Turn Hexapode on
def shexaoff '{
local _list[] _u _a
if ( $# == 0 ) {
_list=SHEXA_SOCKET
} else {
_list[$1]=" "
}
for ( _u in _list ) {
SHEXA_ARR[_u]["disable"]=1
}
}'
#%IU% [unit] [par]
#%MDESC%
def _shexa_get_scale(par,unit) '{
local _i _j
for ( _i=0;_i<MOTORS;_i++) {
if ( (SHEXA_MOT[_i]["shexa"]==unit) && (length(SHEXA_MOT[_i]["shexa"])!=0) ) {
par[_j++]=SHEXA_MOT[_i]["scale"]
}
}
}'
#%UU% [unit]
#%MDESC%
# Return the status of Hexapods
def shexastatus '{
local _i _r _b _a _u _rep[] _list _mon
if ( $# == 0 ) {
_list=SHEXA_SOCKET
} else {
_list[0]=SHEXA_SOCKET[$1]
}
for ( _u in _list) {
_mon=_shexa_io(_u,"STATE#MOTOR?",1)
printf("%c[%dm",27,1);printf("\tUnit %d : %s %s",_u,SHEXA_SOCKET[_u],index(_mon,"1") != 0 ? "ON" : "OFF");printf("%c[0m",27)
_n=split(_shexa_io(_u,"HELP?",1),_a,"Hexapod ID:")
if (_n>=2) {
_n=split(_a[1],_b,"\r")
printf("\n")
printf("%c[%dm",27,34)
printf("\tUsing Hexapod ID :%s\n",_b[0])
printf("\t%s\n",_b[1])
printf("%c[0m",27)
} else {
printf("shexa: Error reading %s\n",SHEXA_SOKET[_u])
}
_shexa_getpar(_rep,_u,"CFG#REPOBJ")
printf("\t Object reference : x=%.3f y=%.3f z=%.3f rx=%.3f ry=%.3f rz=%.3f\n",_rep[0],_rep[1],_rep[2],_rep[3],_rep[4],_rep[5])
_shexa_getpar(_rep,_u,"CFG#REPUTIL")
printf("\t User reference : x=%.3f y=%.3f z=%.3f rx=%.3f ry=%.3f rz=%.3f\n",_rep[0],_rep[1],_rep[2],_rep[3],_rep[4],_rep[5])
_shexa_get_scale(_rep,_u)
printf("\t Scale factor : %d %d %d %d %d %d\n",_rep[0],_rep[1],_rep[2],_rep[3],_rep[4],_rep[5])
_shexa_getpar(_rep,_u,"CFG#DISPLAY")
printf("\t Controller units : translation %s rotation %s\n",SHEXA_DISP["trans"][_rep[0]],SHEXA_DISP["rot"][_rep[1]])
printf("\n")
_st=_shexa_getstatus_HEXA(_u)
_ans=substr(_shexa_io(_u,"MOV?",1),1,4)
printf(" \t %s status=0x%02d ==> ",_ans,_st)
if ( _st==0 ) {
printf("%c[%dm",27,31);printf("You need to make references (shexaref [unit])");printf("%c[0m",27)
} else {
for (_i=0;_i<11;_i++) {
if ( (_st & ( 1<<_i))==(1<<_i)) {
printf(" | %s", SHEXA_ARR["msgStatus"][_i*2+1])
}
}
}
printf("\n\t %s\n\n",_shexaerror(_u))
}
}'
#%UU% [unit]
#%MDESC%
# Clear errors
def shexaclearerros '{
local _u _list
if ( $# == 0 ) {
_list=SHEXA_SOCKET
} else {
_list[0]=SHEXA_SOCKET[$1]
}
for ( _u in SHEXA_SOCKET) {
_shexa_io(_u,"ERRORCLEAR")
}
}'
#%IU% [unit]
#%MDESC%
#Display hexapod commands
def _shexa_get_help '{
local _u _ans _i _a _n _j
_u=$1
_n = split(_shexa_io(_u,"HELP?",1),_a,"\r")
printf("%c[%dm",27,31)
printf("\tUsing these commands may damage the hardware : \n\tPlease be careful\n")
printf("%c[0m",27)
_j=0
for ( _i=0 ; _i<_n ; _i++ ) {
if ( length(_a[_i])<15) {
printf("%15s%s",_a[_i],_i%4!=0?"":"\n" )
} else { p _a[_i] }
}
}'
#%IU% [unit] [command]
#%MDESC%
# To test speed and status returned by controller.
# [unit] [command]
def _shexa_bench '{
local _u _t ans _ost _I _ans _oans
if ( $# != 2 ) {
printf("Usage : _shexa_bench [unit] [command]\n")
printf(" Ex : _shexa_bench 1 MOV#RELO0,0,0.1,0,0,0\n")
printf(" Ex : _shexa_bench 1 HM#STD\n")
exit
}
_u=$1
_cmd="$2"
_t=time()
_st=_shexa_getstatus_HEXA(_u)
_ans=substr(_shexa_io(_u,"MOV?",1),1,4)
printf("t=%.3f %s status=0x%02x ==> ",time()-_t,_ans,_st)
for (_i=0;_i<11;_i++) {
if ( (_st & ( 1<<_i))==(1<<_i)) {
printf(" | %s", SHEXA_ARR["msgStatus"][_i*2+1])
}
}
printf("\n")
_shexa_io($1,"$2")
_ost=-1
while ( (time()-_t) < 100 ) {
_st=_shexa_getstatus_HEXA(_u)
_ans=substr(_shexa_io(_u,"MOV?",1),1,4)
if ( (_st != _ost) | (_ans!=_oans) ) {
_ost=_st; _oans=_ans
printf("t=%.3f %s status=0x%02x ==> ",time()-_t,_ans,_st)
for (_i=0;_i<11;_i++) {
if ( (_st & ( 1<<_i))==(1<<_i)) {
printf(" | %s", SHEXA_ARR["msgStatus"][_i*2+1])
}
}
printf("\n")
}
}
}'
#%MACROS%
#%IMACROS%
#%LOG%
#$Revision: 1.1 $
#$Log: shexamacromotor.mac,v $
#Revision 1.1 2012/05/07 16:07:32 claustre
#Initial revision
#
#
#%AUTHOR% R.Homs / O.Ulrich 2012
#%TOC%
|
