238 lines
11 KiB
XML
238 lines
11 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">
|
|
<Locals>
|
|
<Local name="rx" type="string" xsi:type="array" size="1" />
|
|
<Local name="tx" type="string" xsi:type="array" size="1" />
|
|
<Local name="rec_val" type="num" xsi:type="array" size="1" />
|
|
<Local name="rec_count" type="num" 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="index" type="num" xsi:type="array" size="1" />
|
|
<Local name="cmd" type="string" xsi:type="array" size="1" />
|
|
<Local name="args" type="string" xsi:type="array" size="10" />
|
|
<Local name="arg" type="num" xsi:type="array" size="1" />
|
|
<Local name="ex" type="string" xsi:type="array" size="1" />
|
|
<Local name="array_separator" type="string" xsi:type="array" size="1" />
|
|
<Local name="last_id" type="string" xsi:type="array" size="1" />
|
|
<Local name="ret" type="num" 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="count" type="num" xsi:type="array" size="1" />
|
|
<Local name="ok" type="bool" xsi:type="array" size="1" />
|
|
</Locals>
|
|
<Code><![CDATA[begin
|
|
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]))
|
|
else
|
|
tx=tx+"0"+array_separator
|
|
endIf
|
|
|
|
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 "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"
|
|
return
|
|
break
|
|
case -2
|
|
ex = "The variable library does not exist"
|
|
return
|
|
break
|
|
case -3
|
|
ex = "The index is out of range"
|
|
return
|
|
break
|
|
case -4
|
|
ex = "The data's type does not match the variable's type"
|
|
return
|
|
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
|
|
|
|
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> |