61 lines
1.9 KiB
XML
61 lines
1.9 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="comModbus">
|
|
<Locals>
|
|
<Local name="l_nJointForce" type="num" xsi:type="array" size="6" />
|
|
<Local name="i" type="num" xsi:type="array" size="1" />
|
|
<Local name="cmd" type="num" xsi:type="array" size="1" />
|
|
<Local name="req" type="bool" xsi:type="array" size="1" />
|
|
<Local name="ret" type="num" xsi:type="array" size="32" />
|
|
<Local name="args" type="num" xsi:type="array" size="32" />
|
|
<Local name="error" type="num" xsi:type="array" size="1" />
|
|
<Local name="aux" type="num" xsi:type="array" size="1" />
|
|
</Locals>
|
|
<Code><![CDATA[begin
|
|
req = false
|
|
aioSet(wAcknowlege,0)
|
|
aioSet(wReturn,0)
|
|
while true
|
|
//Record the joint force (comment out next line for use with expansion add-on)
|
|
getJointForce(l_nJointForce)
|
|
for i=0 to 5
|
|
aux = l_nJointForce[i]*100
|
|
if aux<0
|
|
//aux = 65536 + aux
|
|
aux = - aux
|
|
endIf
|
|
aioSet(JF[i], aux)
|
|
endFor
|
|
|
|
|
|
if !req
|
|
if aioGet(wRequest) != 0
|
|
req = true
|
|
cmd = aioGet(wCommand)
|
|
args = aioGet(wArguments)
|
|
error = 1
|
|
for i=0 to size(ret)-1
|
|
ret[i]=0
|
|
endFor
|
|
call onCommandModbus(cmd, args, ret, error)
|
|
for i=0 to size(wReturn)-1
|
|
aioSet(wReturn[i],ret[i])
|
|
endFor
|
|
if error == 0
|
|
error = 1
|
|
endIf
|
|
aioSet(wAcknowlege,error)
|
|
endIf
|
|
else
|
|
if aioGet(wRequest) == 0
|
|
req = false
|
|
aioSet(wAcknowlege,0)
|
|
endIf
|
|
endIf
|
|
|
|
|
|
delay(0)
|
|
endWhile
|
|
end]]></Code>
|
|
</Program>
|
|
</Programs> |