50 lines
1.4 KiB
XML
50 lines
1.4 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="moveScanner">
|
|
<Code><![CDATA[begin
|
|
|
|
tcp_ret=-100
|
|
|
|
//if Robot already in position
|
|
if distance(here(tSuna,world),pScanHome)<nTolerance
|
|
movel(pScanHome,tSuna,mDewarEnter)
|
|
waitEndMove()
|
|
tcp_ret=1
|
|
|
|
//if Robot is near Dewar Home Position
|
|
elseIf distance(here(tSuna,world),pDewarHome)<nTolerance
|
|
movel(pDewarHome,tSuna,mDewarEnter)
|
|
movel(pScanHome,tSuna,mDewarEnter)
|
|
waitEndMove()
|
|
tcp_ret=2
|
|
|
|
//if Robot is near Dewar Wait Position
|
|
elseIf distance(here(tSuna,world),pDewarWait)<nTolerance
|
|
movel(pDewarWait,tSuna,mDewarEnter)
|
|
movel(pDewarHome,tSuna,mDewarEnter)
|
|
movel(pScanHome,tSuna,mDewarEnter)
|
|
waitEndMove()
|
|
tcp_ret=3
|
|
|
|
//if Robot is near Gonio Home Position
|
|
elseIf distance(here(tSuna,world),pGonioHome)<nTolerance
|
|
movel(pGonioHome,tSuna,mDewarEnter)
|
|
movej(pDewarHome,tSuna,mDewarEnter)
|
|
movel(pScanHome,tSuna,mDewarEnter)
|
|
waitEndMove()
|
|
tcp_ret=4
|
|
|
|
//if Robot is near Park Position
|
|
elseIf distance(here(tSuna,world),pPark)<nTolerance
|
|
movel(pPark,tSuna,mDewarEnter)
|
|
movej(pScanHome,tSuna,mDewarEnter)
|
|
waitEndMove()
|
|
tcp_ret=4
|
|
|
|
//if Robot is in Dewar Wait
|
|
else
|
|
tcp_ret=-1
|
|
endIf
|
|
end]]></Code>
|
|
</Program>
|
|
</Programs> |