301 lines
10 KiB
XML
301 lines
10 KiB
XML
<?xml version="1.0" encoding="utf-8" ?>
|
|
<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2" >
|
|
<Program name="comTcp" access="private" >
|
|
<Locals>
|
|
<Local name="arg" type="num" xsi:type="array" size="1" />
|
|
<Local name="args" type="string" xsi:type="array" size="10" />
|
|
<Local name="array_separator" type="string" xsi:type="array" size="1" />
|
|
<Local name="aux" type="num" xsi:type="array" size="1" />
|
|
<Local name="auxb" type="bool" xsi:type="array" size="1" />
|
|
<Local name="cmd" type="string" xsi:type="array" size="1" />
|
|
<Local name="count" type="num" xsi:type="array" size="1" />
|
|
<Local name="ex" type="string" xsi:type="array" size="1" />
|
|
<Local name="index" type="num" xsi:type="array" size="1" />
|
|
<Local name="j" type="joint" xsi:type="array" size="1" />
|
|
<Local name="last_id" type="string" xsi:type="array" size="1" />
|
|
<Local name="msg" type="string" xsi:type="array" size="1" />
|
|
<Local name="msg_id" type="string" xsi:type="array" size="1" />
|
|
<Local name="ok" type="bool" xsi:type="array" size="1" />
|
|
<Local name="p" type="point" xsi:type="array" size="1" />
|
|
<Local name="paux" type="point" xsi:type="array" size="1" />
|
|
<Local name="precision" type="string" xsi:type="array" size="1" />
|
|
<Local name="rec_count" type="num" xsi:type="array" size="1" />
|
|
<Local name="rec_val" type="num" xsi:type="array" size="1" />
|
|
<Local name="ret" type="num" xsi:type="array" size="1" />
|
|
<Local name="rx" type="string" xsi:type="array" size="1" />
|
|
<Local name="tx" type="string" xsi:type="array" size="1" />
|
|
</Locals>
|
|
<Code><![CDATA[begin
|
|
//clean screen
|
|
|
|
|
|
rx=""
|
|
last_id=""
|
|
sioCtrl(sSio,"timeout",1)
|
|
sioCtrl(sSio,"endOfString",10)
|
|
array_separator="|"
|
|
while true
|
|
while sioGet(sSio,rec_val)>0
|
|
if rec_val>31
|
|
rx=rx+chr(rec_val)
|
|
elseIf rec_val==10
|
|
if len(rx)<4
|
|
rx=""
|
|
else
|
|
msg_id=left(rx,4)
|
|
msg=right(rx,len(rx)-4)
|
|
rx=""
|
|
//If same id, repeat last answer
|
|
if msg_id!=last_id
|
|
for index=0 to 9
|
|
args[index]=""
|
|
endFor
|
|
index=find(msg," ")
|
|
if index<0
|
|
cmd=msg
|
|
else
|
|
cmd=left(msg,index)
|
|
msg=right(msg,len(msg)-index-1)
|
|
arg=0
|
|
while (len(msg)>0 and (arg<10))
|
|
index=find(msg,array_separator)
|
|
if index<0
|
|
args[arg]=msg
|
|
msg=""
|
|
else
|
|
args[arg]=left(msg,index)
|
|
msg=right(msg,len(msg)-index-1)
|
|
endIf
|
|
arg=arg+1
|
|
endWhile
|
|
endIf
|
|
tx=""
|
|
ex=""
|
|
//General commands
|
|
switch (cmd)
|
|
case "eval"
|
|
tx=$exec(args[0])
|
|
break
|
|
|
|
case "get_status"
|
|
aux=workingMode(count)
|
|
tx=toString("",aux)+array_separator
|
|
tx=tx+toString("",count)+array_separator
|
|
if isPowered()
|
|
tx=tx+"1"+array_separator
|
|
else
|
|
tx=tx+"0"+array_separator
|
|
endIf
|
|
aux=getMonitorSpeed()
|
|
tx=tx+toString("",aux)+array_separator
|
|
if isEmpty()
|
|
tx=tx+"1"+array_separator
|
|
else
|
|
tx=tx+"0"+array_separator
|
|
endIf
|
|
if isSettled()
|
|
tx=tx+"1"+array_separator
|
|
else
|
|
tx=tx+"0"+array_separator
|
|
endIf
|
|
//Task status
|
|
if len(args[0])>0
|
|
tx=tx+toString("",taskStatus(args[0]))+array_separator
|
|
else
|
|
tx=tx+"0"+array_separator
|
|
endIf
|
|
tx=tx+toString("",tcp_ret)+array_separator
|
|
j=herej()
|
|
//Flange position
|
|
//p=jointToPoint(flange,world, herej())
|
|
|
|
//Tool position
|
|
//$exec("p=jointToPoint(" + args[1] + "," + args[2] + ", herej())")
|
|
p=jointToPoint(tcp_curtool,world,herej())
|
|
|
|
precision="0.2"
|
|
tx=tx+toString(precision,j.j1)+array_separator
|
|
tx=tx+toString(precision,j.j2)+array_separator
|
|
tx=tx+toString(precision,j.j3)+array_separator
|
|
tx=tx+toString(precision,j.j4)+array_separator
|
|
tx=tx+toString(precision,j.j5)+array_separator
|
|
tx=tx+toString(precision,j.j6)+array_separator
|
|
tx=tx+toString(precision,p.trsf.x)+array_separator
|
|
tx=tx+toString(precision,p.trsf.y)+array_separator
|
|
tx=tx+toString(precision,p.trsf.z)+array_separator
|
|
tx=tx+toString(precision,p.trsf.rx)+array_separator
|
|
tx=tx+toString(precision,p.trsf.ry)+array_separator
|
|
tx=tx+toString(precision,p.trsf.rz)+array_separator
|
|
|
|
if tcp_events[0]!=""
|
|
tx=tx+tcp_events[0]
|
|
for index=0 to 8
|
|
tcp_events[index]=tcp_events[index+1]
|
|
endFor
|
|
tcp_events[9]=""
|
|
endIf
|
|
break
|
|
|
|
case "task_sts"
|
|
tx=""
|
|
for index=0 to (len(args)-1)
|
|
if len(args[index])>0
|
|
tx=tx+toString("",taskStatus(args[index]))+array_separator
|
|
endIf
|
|
endFor
|
|
break
|
|
|
|
case "dist_pnt"
|
|
//tolerance
|
|
p=jointToPoint(tcp_curtool,world,herej())
|
|
tx=""
|
|
for index=0 to (len(args)-1)
|
|
if len(args[index])>0
|
|
ret=getData(args[index],paux)
|
|
tx=tx+toString(".4",distance(p,paux))+array_separator
|
|
endIf
|
|
endFor
|
|
break
|
|
|
|
case "get_var","get_bool"
|
|
if cmd=="get_var"
|
|
ret=getData(args[0],aux)
|
|
else
|
|
ret=getData(args[0],auxb)
|
|
endIf
|
|
switch (ret)
|
|
case -1
|
|
ex="The variable does not exists"
|
|
break
|
|
case -2
|
|
ex="The variable library does not exist"
|
|
break
|
|
case -3
|
|
ex="The index is out of range"
|
|
break
|
|
case -4
|
|
ex="The data's type does not match the variable's type"
|
|
break
|
|
default
|
|
if cmd=="get_var"
|
|
tx=toString(".4",aux)
|
|
else
|
|
if auxb
|
|
tx="1"
|
|
else
|
|
tx="0"
|
|
endIf
|
|
endIf
|
|
break
|
|
endSwitch
|
|
break
|
|
|
|
case "get_arr"
|
|
toNum(args[1],count,ok)
|
|
for index=0 to count
|
|
ret=getData(args[0]+"["+toString("",index)+"]",aux)
|
|
switch (ret)
|
|
case -1
|
|
ex="The variable does not exists"
|
|
break
|
|
case -2
|
|
ex="The variable library does not exist"
|
|
break
|
|
case -3
|
|
ex="The index is out of range"
|
|
break
|
|
case -4
|
|
ex="The data's type does not match the variable's type"
|
|
break
|
|
default
|
|
tx=tx+toString(".4",aux)+"|"
|
|
break
|
|
endSwitch
|
|
endFor
|
|
break
|
|
|
|
//case "get_str"
|
|
// //$exec("tcp_s = " + args[0]) TODO: MAKES THE CONTROLLER TO CRASH!
|
|
// //tx = s
|
|
//break
|
|
|
|
//taskKill can freeze if executed by eval
|
|
case "kill"
|
|
taskKill(args[0])
|
|
break
|
|
case "save"
|
|
aux=libSave()
|
|
tx=toString("",aux)
|
|
break
|
|
case "get_profile"
|
|
tx=getProfile()
|
|
break
|
|
case "set_profile"
|
|
ret=setProfile(args[0],args[1])
|
|
if (ret<0)
|
|
ex="Invalid profile name or password"
|
|
endIf
|
|
break
|
|
case "get_pnt"
|
|
$exec("tcp_p = "+args[0])
|
|
tx=tx+toString(".4",tcp_p.trsf.x)+"|"
|
|
tx=tx+toString(".4",tcp_p.trsf.y)+"|"
|
|
tx=tx+toString(".4",tcp_p.trsf.z)+"|"
|
|
tx=tx+toString(".4",tcp_p.trsf.rx)+"|"
|
|
tx=tx+toString(".4",tcp_p.trsf.ry)+"|"
|
|
tx=tx+toString(".4",tcp_p.trsf.rz)+"|"
|
|
break
|
|
|
|
case "get_jnt"
|
|
$exec("tcp_j = "+args[0])
|
|
tx=tx+toString(".4",tcp_j.j1)+"|"
|
|
tx=tx+toString(".4",tcp_j.j2)+"|"
|
|
tx=tx+toString(".4",tcp_j.j3)+"|"
|
|
tx=tx+toString(".4",tcp_j.j4)+"|"
|
|
tx=tx+toString(".4",tcp_j.j5)+"|"
|
|
tx=tx+toString(".4",tcp_j.j6)+"|"
|
|
break
|
|
|
|
case "get_trf"
|
|
$exec("tcp_t = "+args[0])
|
|
tx=tx+toString(".4",tcp_t.x)+"|"
|
|
tx=tx+toString(".4",tcp_t.y)+"|"
|
|
tx=tx+toString(".4",tcp_t.z)+"|"
|
|
tx=tx+toString(".4",tcp_t.rx)+"|"
|
|
tx=tx+toString(".4",tcp_t.ry)+"|"
|
|
tx=tx+toString(".4",tcp_t.rz)+"|"
|
|
break
|
|
|
|
case "get_help"
|
|
toNum(args[0],aux,ok)
|
|
if ok==true
|
|
tx=help(aux)
|
|
else
|
|
ex="Invalid code: "+args[0]
|
|
endIf
|
|
break
|
|
|
|
default
|
|
//App specific
|
|
call onCommandTcp(cmd,args,tx,ex)
|
|
if ((len(tx)==0) and (len(ex)==0))
|
|
ex="Invalid command: "+cmd
|
|
endIf
|
|
break
|
|
endSwitch
|
|
last_id=msg_id
|
|
endIf
|
|
if len(ex)==0
|
|
sSio=msg_id+tx
|
|
else
|
|
msg_id=replace(msg_id,"*",1,3)
|
|
sSio=msg_id+ex
|
|
endIf
|
|
endIf
|
|
endIf
|
|
endWhile
|
|
delay(0)
|
|
endWhile
|
|
end]]></Code>
|
|
</Program>
|
|
</Programs> |